mirror of https://github.com/gnss-sdr/gnss-sdr
Add rtkpos and its dependencies
This commit is contained in:
Carles Fernandez
parent
ce8869c5c5
commit
52f3eaa373
|
|
@ -25,6 +25,9 @@ set(RTKLIB_LIB_SOURCES
|
|||
rtklib_sbas.cc
|
||||
rtklib_ionex.cc
|
||||
rtklib_pntpos.cc
|
||||
rtklib_ppp.cc
|
||||
rtklib_tides.cc
|
||||
rtklib_lambda.cc
|
||||
rtklib_rtkpos.cc
|
||||
rtklib_conversions.cc
|
||||
)
|
||||
|
|
|
|||
|
|
@ -71,6 +71,9 @@ const double RE_WGS84 = 6378137.0; //!< earth semimajor axis (WGS84)
|
|||
const double FE_WGS84 = (1.0 / 298.257223563); //!< earth flattening (WGS84)
|
||||
|
||||
const double HION = 350000.0; //!< ionosphere height (m)
|
||||
const double PRN_HWBIAS = 1e-6; //!< process noise of h/w bias (m/MHz/sqrt(s))
|
||||
|
||||
const double INT_SWAP_STAT = 86400.0; //!< swap interval of solution status file (s)
|
||||
|
||||
const unsigned int POLYCRC32 = 0xEDB88320u; //!< CRC32 polynomial
|
||||
const unsigned int POLYCRC24Q = 0x1864CFBu; //!< CRC24Q polynomial
|
||||
|
|
@ -106,12 +109,20 @@ const unsigned int TIMES_GPST = 0; //!< time system: gps time
|
|||
const unsigned int TIMES_UTC = 1; //!< time system: utc
|
||||
const unsigned int TIMES_JST = 2; //!< time system: jst
|
||||
|
||||
|
||||
const double ERR_SAAS = 0.3; //!< saastamoinen model error std (m)
|
||||
const double ERR_BRDCI = 0.5; //!< broadcast iono model error factor
|
||||
const double ERR_CBIAS = 0.3; //!< code bias error std (m)
|
||||
const double REL_HUMI = 0.7; //!< relative humidity for saastamoinen model
|
||||
const double GAP_RESION = 120; //!< default gap to reset ionos parameters (ep)
|
||||
|
||||
const int MAXFREQ = 7; //!< max NFREQ
|
||||
|
||||
const int MAXLEAPS = 64; //!< max number of leap seconds table
|
||||
const double DTTOL = 0.005; //!< tolerance of time difference (s)
|
||||
|
||||
const int NFREQ = 3;
|
||||
const int NFREQ = 3; //!< number of carrier frequencies
|
||||
const int NFREQGLO = 2; //!< number of carrier frequencies of GLONASS
|
||||
const int NEXOBS = 0; //!< number of extended obs codes
|
||||
const int MAXANT = 64; //!< max length of station name/antenna type
|
||||
|
||||
|
|
@ -131,6 +142,8 @@ const int SYS_IRN = 0x40; //!< navigation system: IRNS
|
|||
const int SYS_LEO = 0x80; //!< navigation system: LEO
|
||||
const int SYS_ALL = 0xFF; //!< navigation system: all
|
||||
|
||||
|
||||
|
||||
#ifdef ENAGLO
|
||||
const int MINPRNGLO = 1; //!< min satellite slot number of GLONASS
|
||||
const int MAXPRNGLO = 27; //!< max satellite slot number of GLONASS
|
||||
|
|
@ -272,10 +285,18 @@ const double EFACT_BDS = 1.0; //!< error factor: BeiDou
|
|||
const double EFACT_IRN = 1.5; //!< error factor: IRNSS
|
||||
const double EFACT_SBS = 3.0; //!< error factor: SBAS
|
||||
|
||||
const int MAXEXFILE = 1024; //!< max number of expanded files
|
||||
const int MAXEXFILE = 1024; //!< max number of expanded files
|
||||
const double MAXSBSAGEF = 30.0; //!< max age of SBAS fast correction (s)
|
||||
const double MAXSBSAGEL = 1800.0; //!< max age of SBAS long term corr (s)
|
||||
|
||||
const unsigned int ARMODE_OFF = 0; //!< AR mode: off
|
||||
const unsigned int ARMODE_CONT = 1; //!< AR mode: continuous
|
||||
const unsigned int ARMODE_INST = 2; //!< AR mode: instantaneous
|
||||
const unsigned int ARMODE_FIXHOLD = 3; //!< AR mode: fix and hold
|
||||
const unsigned int ARMODE_WLNL = 4; //!< AR mode: wide lane/narrow lane
|
||||
const unsigned int ARMODE_TCAR = 5; //!< AR mode: triple carrier ar
|
||||
|
||||
const int POSOPT_RINEX = 3; //!< pos option: rinex header pos */
|
||||
|
||||
typedef void fatalfunc_t(const char *); //!< fatal callback function type
|
||||
|
||||
|
|
|
|||
|
|
@ -1,57 +1,35 @@
|
|||
/*!
|
||||
* \file rtklib_conversions.cc
|
||||
* \brief GNSS-SDR to RTKLIB data structures conversion functions
|
||||
* \authors <ul>
|
||||
* <li> 2017, Javier Arribas
|
||||
* <li> 2017, Carles Fernandez
|
||||
* <li> 2007-2013, T. Takasu
|
||||
* </ul>
|
||||
*
|
||||
* This is a derived work from RTKLIB http://www.rtklib.com/
|
||||
* The original source code at https://github.com/tomojitakasu/RTKLIB is
|
||||
* released under the BSD 2-clause license with an additional exclusive clause
|
||||
* that does not apply here. This additional clause is reproduced below:
|
||||
*
|
||||
* " The software package includes some companion executive binaries or shared
|
||||
* libraries necessary to execute APs on Windows. These licenses succeed to the
|
||||
* original ones of these software. "
|
||||
*
|
||||
* Neither the executive binaries nor the shared libraries are required by, used
|
||||
* or included in GNSS-SDR.
|
||||
* \author 2017, Javier Arribas
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
* Copyright (C) 2007-2013, T. Takasu
|
||||
* Copyright (C) 2017, Javier Arribas
|
||||
* Copyright (C) 2017, Carles Fernandez
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are
|
||||
* met:
|
||||
* Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
|
||||
* A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
|
||||
* HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
|
||||
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
|
||||
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
|
||||
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
|
||||
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
* GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
*----------------------------------------------------------------------------*/
|
||||
* GNSS-SDR is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "rtklib_conversions.h"
|
||||
|
||||
#include "rtklib_rtkcmn.h"
|
||||
|
||||
obsd_t insert_obs_to_rtklib(obsd_t rtklib_obs, Gnss_Synchro gnss_synchro, int week, int band)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -1,60 +1,37 @@
|
|||
/*!
|
||||
* \file rtklib_conversions.h
|
||||
* \brief GNSS-SDR to RTKLIB data structures conversion functions
|
||||
* \authors <ul>
|
||||
* <li> 2017, Javier Arribas
|
||||
* <li> 2017, Carles Fernandez
|
||||
* <li> 2007-2013, T. Takasu
|
||||
* </ul>
|
||||
*
|
||||
* This is a derived work from RTKLIB http://www.rtklib.com/
|
||||
* The original source code at https://github.com/tomojitakasu/RTKLIB is
|
||||
* released under the BSD 2-clause license with an additional exclusive clause
|
||||
* that does not apply here. This additional clause is reproduced below:
|
||||
*
|
||||
* " The software package includes some companion executive binaries or shared
|
||||
* libraries necessary to execute APs on Windows. These licenses succeed to the
|
||||
* original ones of these software. "
|
||||
*
|
||||
* Neither the executive binaries nor the shared libraries are required by, used
|
||||
* or included in GNSS-SDR.
|
||||
* \author 2017, Javier Arribas
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
* Copyright (C) 2007-2013, T. Takasu
|
||||
* Copyright (C) 2017, Javier Arribas
|
||||
* Copyright (C) 2017, Carles Fernandez
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are
|
||||
* met:
|
||||
* Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
|
||||
* A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
|
||||
* HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
|
||||
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
|
||||
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
|
||||
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
|
||||
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
* GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
*----------------------------------------------------------------------------*/
|
||||
|
||||
* GNSS-SDR is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef GNSS_SDR_RTKLIB_CONVERSIONS_H_
|
||||
#define GNSS_SDR_RTKLIB_CONVERSIONS_H_
|
||||
|
||||
#include "rtklib_rtkcmn.h"
|
||||
#include "rtklib.h"
|
||||
#include "gnss_synchro.h"
|
||||
#include "galileo_ephemeris.h"
|
||||
#include "gps_ephemeris.h"
|
||||
|
|
|
|||
|
|
@ -51,7 +51,9 @@
|
|||
*----------------------------------------------------------------------------*/
|
||||
|
||||
#include "rtklib_ephemeris.h"
|
||||
|
||||
#include "rtklib_rtkcmn.h"
|
||||
#include "rtklib_sbas.h"
|
||||
#include "rtklib_preceph.h"
|
||||
|
||||
/* constants ------------------------------------------------------*/
|
||||
|
||||
|
|
|
|||
|
|
@ -55,9 +55,7 @@
|
|||
#define GNSS_SDR_RTKLIB_EPHEMERIS_H_
|
||||
|
||||
#include "rtklib.h"
|
||||
#include "rtklib_rtkcmn.h"
|
||||
#include "rtklib_sbas.h"
|
||||
#include "rtklib_preceph.h"
|
||||
|
||||
|
||||
double var_uraeph(int ura);
|
||||
double var_urassr(int ura);
|
||||
|
|
|
|||
|
|
@ -58,7 +58,7 @@
|
|||
*----------------------------------------------------------------------------*/
|
||||
|
||||
#include "rtklib_ionex.h"
|
||||
|
||||
#include "rtklib_rtkcmn.h"
|
||||
|
||||
/* get index -----------------------------------------------------------------*/
|
||||
int getindex(double value, const double *range)
|
||||
|
|
|
|||
|
|
@ -60,7 +60,7 @@
|
|||
#ifndef GNSS_SDR_RTKLIB_IONEX_H_
|
||||
#define GNSS_SDR_RTKLIB_IONEX_H_
|
||||
|
||||
#include "rtklib_rtkcmn.h"
|
||||
#include "rtklib.h"
|
||||
|
||||
const double VAR_NOTEC = 30.0 * 30.0; /* variance of no tec */
|
||||
const double MIN_EL = 0.0; /* min elevation angle (rad) */
|
||||
|
|
|
|||
|
|
@ -0,0 +1,327 @@
|
|||
/*!
|
||||
* \file rtklib_lambda.cc
|
||||
* \brief Integer ambiguity resolution
|
||||
* \authors <ul>
|
||||
* <li> 2007-2008, T. Takasu
|
||||
* <li> 2017, Javier Arribas
|
||||
* <li> 2017, Carles Fernandez
|
||||
* </ul>
|
||||
*
|
||||
* This is a derived work from RTKLIB http://www.rtklib.com/
|
||||
* The original source code at https://github.com/tomojitakasu/RTKLIB is
|
||||
* released under the BSD 2-clause license with an additional exclusive clause
|
||||
* that does not apply here. This additional clause is reproduced below:
|
||||
*
|
||||
* " The software package includes some companion executive binaries or shared
|
||||
* libraries necessary to execute APs on Windows. These licenses succeed to the
|
||||
* original ones of these software. "
|
||||
*
|
||||
* Neither the executive binaries nor the shared libraries are required by, used
|
||||
* or included in GNSS-SDR.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
* Copyright (C) 2007-2008, T. Takasu
|
||||
* Copyright (C) 2017, Javier Arribas
|
||||
* Copyright (C) 2017, Carles Fernandez
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are
|
||||
* met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
|
||||
* A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
|
||||
* HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
|
||||
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
|
||||
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
|
||||
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
|
||||
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
*----------------------------------------------------------------------------*/
|
||||
|
||||
#include "rtklib_lambda.h"
|
||||
#include "rtklib_rtkcmn.h"
|
||||
|
||||
/* LD factorization (Q=L'*diag(D)*L) -----------------------------------------*/
|
||||
int LD(int n, const double *Q, double *L, double *D)
|
||||
{
|
||||
int i,j,k,info = 0;
|
||||
double a,*A = mat(n,n);
|
||||
|
||||
memcpy(A,Q,sizeof(double)*n*n);
|
||||
for (i = n-1; i >= 0; i--)
|
||||
{
|
||||
if ((D[i] = A[i+i*n])<=0.0) {info = -1; break;}
|
||||
a = sqrt(D[i]);
|
||||
for (j = 0; j<=i; j++) L[i+j*n] = A[i+j*n]/a;
|
||||
for (j = 0; j<=i-1; j++) for (k = 0;k<=j;k++) A[j+k*n] -= L[i+k*n]*L[i+j*n];
|
||||
for (j = 0; j<=i; j++) L[i+j*n] /= L[i+i*n];
|
||||
}
|
||||
free(A);
|
||||
if (info) fprintf(stderr,"%s : LD factorization error\n",__FILE__);
|
||||
return info;
|
||||
}
|
||||
|
||||
|
||||
/* integer gauss transformation ----------------------------------------------*/
|
||||
void gauss(int n, double *L, double *Z, int i, int j)
|
||||
{
|
||||
int k,mu;
|
||||
|
||||
if ((mu = (int)ROUND_LAMBDA(L[i+j*n]))!=0)
|
||||
{
|
||||
for (k = i; k<n; k++) L[k+n*j] -= (double)mu*L[k+i*n];
|
||||
for (k = 0; k<n; k++) Z[k+n*j] -= (double)mu*Z[k+i*n];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/* permutations --------------------------------------------------------------*/
|
||||
void perm(int n, double *L, double *D, int j, double del, double *Z)
|
||||
{
|
||||
int k;
|
||||
double eta,lam,a0,a1;
|
||||
|
||||
eta = D[j]/del;
|
||||
lam = D[j+1]*L[j+1+j*n]/del;
|
||||
D[j] = eta*D[j+1]; D[j+1] = del;
|
||||
for (k = 0; k<=j-1; k++)
|
||||
{
|
||||
a0 = L[j+k*n];
|
||||
a1 = L[j+1+k*n];
|
||||
L[j+k*n] = -L[j+1+j*n]*a0+a1;
|
||||
L[j+1+k*n] = eta*a0+lam*a1;
|
||||
}
|
||||
L[j+1+j*n] = lam;
|
||||
for (k = j+2;k<n;k++) SWAP_LAMBDA(L[k+j*n],L[k+(j+1)*n]);
|
||||
for (k = 0;k<n;k++) SWAP_LAMBDA(Z[k+j*n],Z[k+(j+1)*n]);
|
||||
}
|
||||
|
||||
|
||||
/* lambda reduction (z=Z'*a, Qz=Z'*Q*Z=L'*diag(D)*L) (ref.[1]) ---------------*/
|
||||
void reduction(int n, double *L, double *D, double *Z)
|
||||
{
|
||||
int i,j,k;
|
||||
double del;
|
||||
|
||||
j = n-2; k = n-2;
|
||||
while (j >= 0)
|
||||
{
|
||||
if (j<=k) for (i = j+1; i<n; i++) gauss(n,L,Z,i,j);
|
||||
del = D[j]+L[j+1+j*n]*L[j+1+j*n]*D[j+1];
|
||||
if (del+1E-6<D[j+1])
|
||||
{ /* compared considering numerical error */
|
||||
perm(n,L,D,j,del,Z);
|
||||
k = j; j = n-2;
|
||||
}
|
||||
else j--;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/* modified lambda (mlambda) search (ref. [2]) -------------------------------*/
|
||||
int search(int n, int m, const double *L, const double *D,
|
||||
const double *zs, double *zn, double *s)
|
||||
{
|
||||
int i,j,k,c,nn = 0,imax = 0;
|
||||
double newdist,maxdist = 1E99,y;
|
||||
double *S = zeros(n,n),*dist = mat(n,1),*zb = mat(n,1),*z = mat(n,1),*step = mat(n,1);
|
||||
|
||||
k = n-1; dist[k] = 0.0;
|
||||
zb[k] = zs[k];
|
||||
z[k] = ROUND_LAMBDA(zb[k]);
|
||||
y = zb[k]-z[k];
|
||||
step[k] = SGN_LAMBDA(y);
|
||||
for (c = 0; c<LOOPMAX; c++)
|
||||
{
|
||||
newdist = dist[k]+y*y/D[k];
|
||||
if (newdist<maxdist)
|
||||
{
|
||||
if (k!=0)
|
||||
{
|
||||
dist[--k] = newdist;
|
||||
for (i = 0; i<=k; i++)
|
||||
S[k+i*n] = S[k+1+i*n]+(z[k+1]-zb[k+1])*L[k+1+i*n];
|
||||
zb[k] = zs[k]+S[k+k*n];
|
||||
z[k] = ROUND_LAMBDA(zb[k]); y = zb[k]-z[k]; step[k] = SGN_LAMBDA(y);
|
||||
}
|
||||
else
|
||||
{
|
||||
if (nn<m)
|
||||
{
|
||||
if (nn == 0||newdist>s[imax]) imax = nn;
|
||||
for (i = 0; i<n; i++) zn[i+nn*n] = z[i];
|
||||
s[nn++] = newdist;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (newdist<s[imax])
|
||||
{
|
||||
for (i = 0; i<n; i++) zn[i+imax*n] = z[i];
|
||||
s[imax] = newdist;
|
||||
for (i = imax = 0; i<m; i++) if (s[imax]<s[i]) imax = i;
|
||||
}
|
||||
maxdist = s[imax];
|
||||
}
|
||||
z[0] += step[0]; y = zb[0]-z[0]; step[0] = -step[0]-SGN_LAMBDA(step[0]);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (k == n-1) break;
|
||||
else
|
||||
{
|
||||
k++;
|
||||
z[k] += step[k]; y = zb[k]-z[k]; step[k] = -step[k]-SGN_LAMBDA(step[k]);
|
||||
}
|
||||
}
|
||||
}
|
||||
for (i = 0; i<m-1; i++)
|
||||
{ /* sort by s */
|
||||
for (j = i+1; j<m; j++)
|
||||
{
|
||||
if (s[i]<s[j]) continue;
|
||||
SWAP_LAMBDA(s[i],s[j]);
|
||||
for (k = 0; k<n; k++) SWAP_LAMBDA(zn[k+i*n],zn[k+j*n]);
|
||||
}
|
||||
}
|
||||
free(S); free(dist); free(zb); free(z); free(step);
|
||||
|
||||
if (c >= LOOPMAX)
|
||||
{
|
||||
fprintf(stderr,"%s : search loop count overflow\n",__FILE__);
|
||||
return -1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
/* lambda/mlambda integer least-square estimation ------------------------------
|
||||
* integer least-square estimation. reduction is performed by lambda (ref.[1]),
|
||||
* and search by mlambda (ref.[2]).
|
||||
* args : int n I number of float parameters
|
||||
* int m I number of fixed solutions
|
||||
* double *a I float parameters (n x 1)
|
||||
* double *Q I covariance matrix of float parameters (n x n)
|
||||
* double *F O fixed solutions (n x m)
|
||||
* double *s O sum of squared residulas of fixed solutions (1 x m)
|
||||
* return : status (0:ok,other:error)
|
||||
* notes : matrix stored by column-major order (fortran convension)
|
||||
*-----------------------------------------------------------------------------*/
|
||||
int lambda(int n, int m, const double *a, const double *Q, double *F,
|
||||
double *s)
|
||||
{
|
||||
int info;
|
||||
double *L,*D,*Z,*z,*E;
|
||||
|
||||
if (n<=0||m<=0) return -1;
|
||||
L = zeros(n,n);
|
||||
D = mat(n,1);
|
||||
Z = eye(n);
|
||||
z = mat(n,1);
|
||||
E = mat(n,m);
|
||||
|
||||
/* LD factorization */
|
||||
if (!(info = LD(n,Q,L,D)))
|
||||
{
|
||||
|
||||
/* lambda reduction */
|
||||
reduction(n,L,D,Z);
|
||||
matmul("TN",n,1,n,1.0,Z,a,0.0,z); /* z=Z'*a */
|
||||
|
||||
/* mlambda search */
|
||||
if (!(info = search(n,m,L,D,z,E,s)))
|
||||
{
|
||||
|
||||
info = solve("T",Z,E,n,m,F); /* F=Z'\E */
|
||||
}
|
||||
}
|
||||
free(L); free(D); free(Z); free(z); free(E);
|
||||
return info;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/* lambda reduction ------------------------------------------------------------
|
||||
* reduction by lambda (ref [1]) for integer least square
|
||||
* args : int n I number of float parameters
|
||||
* double *Q I covariance matrix of float parameters (n x n)
|
||||
* double *Z O lambda reduction matrix (n x n)
|
||||
* return : status (0:ok,other:error)
|
||||
*-----------------------------------------------------------------------------*/
|
||||
int lambda_reduction(int n, const double *Q, double *Z)
|
||||
{
|
||||
double *L,*D;
|
||||
int i,j,info;
|
||||
|
||||
if (n<=0) return -1;
|
||||
|
||||
L = zeros(n,n);
|
||||
D = mat(n,1);
|
||||
|
||||
for (i = 0; i<n; i++) for (j = 0; j<n; j++)
|
||||
{
|
||||
Z[i+j*n] = i == j ? 1.0 : 0.0;
|
||||
}
|
||||
/* LD factorization */
|
||||
if ((info = LD(n,Q,L,D)))
|
||||
{
|
||||
free(L);
|
||||
free(D);
|
||||
return info;
|
||||
}
|
||||
/* lambda reduction */
|
||||
reduction(n,L,D,Z);
|
||||
|
||||
free(L);
|
||||
free(D);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
/* mlambda search --------------------------------------------------------------
|
||||
* search by mlambda (ref [2]) for integer least square
|
||||
* args : int n I number of float parameters
|
||||
* int m I number of fixed solutions
|
||||
* double *a I float parameters (n x 1)
|
||||
* double *Q I covariance matrix of float parameters (n x n)
|
||||
* double *F O fixed solutions (n x m)
|
||||
* double *s O sum of squared residulas of fixed solutions (1 x m)
|
||||
* return : status (0:ok,other:error)
|
||||
*-----------------------------------------------------------------------------*/
|
||||
int lambda_search(int n, int m, const double *a, const double *Q,
|
||||
double *F, double *s)
|
||||
{
|
||||
double *L,*D;
|
||||
int info;
|
||||
|
||||
if (n<=0||m<=0) return -1;
|
||||
|
||||
L = zeros(n,n);
|
||||
D = mat(n,1);
|
||||
|
||||
/* LD factorization */
|
||||
if ((info = LD(n,Q,L,D)))
|
||||
{
|
||||
free(L);
|
||||
free(D);
|
||||
return info;
|
||||
}
|
||||
/* mlambda search */
|
||||
info = search(n,m,L,D,a,F,s);
|
||||
|
||||
free(L);
|
||||
free(D);
|
||||
return info;
|
||||
}
|
||||
|
|
@ -0,0 +1,87 @@
|
|||
/*!
|
||||
* \file rtklib_lambda.h
|
||||
* \brief Integer ambiguity resolution
|
||||
* \authors <ul>
|
||||
* <li> 2007-2008, T. Takasu
|
||||
* <li> 2017, Javier Arribas
|
||||
* <li> 2017, Carles Fernandez
|
||||
* </ul>
|
||||
*
|
||||
* This is a derived work from RTKLIB http://www.rtklib.com/
|
||||
* The original source code at https://github.com/tomojitakasu/RTKLIB is
|
||||
* released under the BSD 2-clause license with an additional exclusive clause
|
||||
* that does not apply here. This additional clause is reproduced below:
|
||||
*
|
||||
* " The software package includes some companion executive binaries or shared
|
||||
* libraries necessary to execute APs on Windows. These licenses succeed to the
|
||||
* original ones of these software. "
|
||||
*
|
||||
* Neither the executive binaries nor the shared libraries are required by, used
|
||||
* or included in GNSS-SDR.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
* Copyright (C) 2007-2008, T. Takasu
|
||||
* Copyright (C) 2017, Javier Arribas
|
||||
* Copyright (C) 2017, Carles Fernandez
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are
|
||||
* met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
|
||||
* A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
|
||||
* HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
|
||||
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
|
||||
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
|
||||
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
|
||||
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* References:
|
||||
* [1] P.J.G.Teunissen, The least-square ambiguity decorrelation adjustment:
|
||||
* a method for fast GPS ambiguity estimation, J.Geodesy, Vol.70, 65-82,
|
||||
* 1995
|
||||
* [2] X.-W.Chang, X.Yang, T.Zhou, MLAMBDA: A modified LAMBDA method for
|
||||
* integer least-squares estimation, J.Geodesy, Vol.79, 552-565, 2005
|
||||
*
|
||||
*----------------------------------------------------------------------------*/
|
||||
|
||||
#ifndef GNSS_SDR_RTKLIB_LAMBDA_H_
|
||||
#define GNSS_SDR_RTKLIB_LAMBDA_H_
|
||||
|
||||
|
||||
#include "rtklib.h"
|
||||
|
||||
/* constants/macros ----------------------------------------------------------*/
|
||||
const int LOOPMAX = 10000; /* maximum count of search loop */
|
||||
#define SGN_LAMBDA(x) ((x)<=0.0?-1.0:1.0)
|
||||
#define ROUND_LAMBDA(x) (floor((x)+0.5))
|
||||
#define SWAP_LAMBDA(x,y) do {double tmp_; tmp_=x; x=y; y=tmp_;} while (0)
|
||||
|
||||
int LD(int n, const double *Q, double *L, double *D);
|
||||
void gauss(int n, double *L, double *Z, int i, int j);
|
||||
void perm(int n, double *L, double *D, int j, double del, double *Z);
|
||||
void reduction(int n, double *L, double *D, double *Z);
|
||||
int search(int n, int m, const double *L, const double *D,
|
||||
const double *zs, double *zn, double *s);
|
||||
|
||||
int lambda(int n, int m, const double *a, const double *Q, double *F, double *s);
|
||||
|
||||
int lambda_reduction(int n, const double *Q, double *Z);
|
||||
|
||||
int lambda_search(int n, int m, const double *a, const double *Q,
|
||||
double *F, double *s);
|
||||
|
||||
|
||||
#endif
|
||||
|
|
@ -51,7 +51,9 @@
|
|||
*----------------------------------------------------------------------------*/
|
||||
|
||||
#include "rtklib_pntpos.h"
|
||||
|
||||
#include "rtklib_ephemeris.h"
|
||||
#include "rtklib_ionex.h"
|
||||
#include "rtklib_sbas.h"
|
||||
|
||||
/* pseudorange measurement error variance ------------------------------------*/
|
||||
double varerr(const prcopt_t *opt, double el, int sys)
|
||||
|
|
|
|||
|
|
@ -55,18 +55,13 @@
|
|||
|
||||
#include "rtklib.h"
|
||||
#include "rtklib_rtkcmn.h"
|
||||
#include "rtklib_ephemeris.h"
|
||||
#include "rtklib_ionex.h"
|
||||
|
||||
/* constants -----------------------------------------------------------------*/
|
||||
const int NX = 4 + 3; //!< # of estimated parameters
|
||||
const int MAXITR = 10; //!< max number of iteration for point pos
|
||||
const double ERR_ION = 5.0; //!< ionospheric delay std (m)
|
||||
const double ERR_TROP = 3.0; //!< tropspheric delay std (m)
|
||||
const double ERR_SAAS = 0.3; //!< saastamoinen model error std (m)
|
||||
const double ERR_BRDCI = 0.5; //!< broadcast iono model error factor
|
||||
const double ERR_CBIAS = 0.3; //!< code bias error std (m)
|
||||
const double REL_HUMI = 0.7; //!< relative humidity for saastamoinen model
|
||||
|
||||
|
||||
/* pseudorange measurement error variance ------------------------------------*/
|
||||
double varerr(const prcopt_t *opt, double el, int sys);
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,199 @@
|
|||
/*!
|
||||
* \file rtklib_ppp.h
|
||||
* \brief Precise Point Positioning
|
||||
* \authors <ul>
|
||||
* <li> 2007-2008, T. Takasu
|
||||
* <li> 2017, Javier Arribas
|
||||
* <li> 2017, Carles Fernandez
|
||||
* </ul>
|
||||
*
|
||||
* This is a derived work from RTKLIB http://www.rtklib.com/
|
||||
* The original source code at https://github.com/tomojitakasu/RTKLIB is
|
||||
* released under the BSD 2-clause license with an additional exclusive clause
|
||||
* that does not apply here. This additional clause is reproduced below:
|
||||
*
|
||||
* " The software package includes some companion executive binaries or shared
|
||||
* libraries necessary to execute APs on Windows. These licenses succeed to the
|
||||
* original ones of these software. "
|
||||
*
|
||||
* Neither the executive binaries nor the shared libraries are required by, used
|
||||
* or included in GNSS-SDR.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
* Copyright (C) 2007-2008, T. Takasu
|
||||
* Copyright (C) 2017, Javier Arribas
|
||||
* Copyright (C) 2017, Carles Fernandez
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are
|
||||
* met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
|
||||
* A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
|
||||
* HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
|
||||
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
|
||||
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
|
||||
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
|
||||
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
*----------------------------------------------------------------------------*/
|
||||
|
||||
|
||||
#ifndef GNSS_SDR_RTKLIB_PPP_H_
|
||||
#define GNSS_SDR_RTKLIB_PPP_H_
|
||||
|
||||
#include "rtklib.h"
|
||||
|
||||
#define MAX_PPP(x,y) ((x)>(y)?(x):(y))
|
||||
#define ROUND_PPP(x) (int)floor((x)+0.5)
|
||||
|
||||
const int MAX_ITER = 8; /* max number of iterations */
|
||||
const double MAX_STD_FIX = 0.15; /* max std-dev (3d) to fix solution */
|
||||
const int MIN_NSAT_SOL = 4; /* min satellite number for solution */
|
||||
const double THRES_REJECT = 4.0; /* reject threshold of posfit-res (sigma) */
|
||||
|
||||
const double THRES_MW_JUMP = 10.0;
|
||||
|
||||
const double VAR_POS_PPP = std::pow(60.0, 2.0); /* init variance receiver position (m^2) */
|
||||
const double VAR_CLK = std::pow(60.0, 2.0); /* init variance receiver clock (m^2) */
|
||||
const double VAR_ZTD = std::pow( 0.6, 2.0); /* init variance ztd (m^2) */
|
||||
const double VAR_GRA_PPP = std::pow(0.01, 2.0); /* init variance gradient (m^2) */
|
||||
const double VAR_DCB = std::pow(30.0, 2.0); /* init variance dcb (m^2) */
|
||||
const double VAR_BIAS = std::pow(60.0, 2.0); /* init variance phase-bias (m^2) */
|
||||
const double VAR_IONO = std::pow(60.0, 2.0); /* init variance iono-delay */
|
||||
const double VAR_GLO_IFB = std::pow( 0.6, 2.0); /* variance of glonass ifb */
|
||||
|
||||
const double EFACT_GPS_L5 = 10.0; /* error factor of GPS/QZS L5 */
|
||||
|
||||
const double MUDOT_GPS = (0.00836*D2R); /* average angular velocity GPS (rad/s) */
|
||||
const double MUDOT_GLO = (0.00888*D2R); /* average angular velocity GLO (rad/s) */
|
||||
const double EPS0_GPS = (13.5*D2R); /* max shadow crossing angle GPS (rad) */
|
||||
const double EPS0_GLO = (14.2*D2R); /* max shadow crossing angle GLO (rad) */
|
||||
const double T_POSTSHADOW = 1800.0; /* post-shadow recovery time (s) */
|
||||
const double QZS_EC_BETA = 20.0; /* max beta angle for qzss Ec (deg) */
|
||||
|
||||
/* number and index of states */
|
||||
#define NF_PPP(opt) ((opt)->ionoopt==IONOOPT_IFLC?1:(opt)->nf)
|
||||
#define NP_PPP(opt) ((opt)->dynamics?9:3)
|
||||
#define NC_PPP(opt) (NSYS)
|
||||
#define NT_PPP(opt) ((opt)->tropopt<TROPOPT_EST?0:((opt)->tropopt==TROPOPT_EST?1:3))
|
||||
#define NI_PPP(opt) ((opt)->ionoopt==IONOOPT_EST?MAXSAT:0)
|
||||
#define ND_PPP(opt) ((opt)->nf>=3?1:0)
|
||||
#define NR_PPP(opt) (NP_PPP(opt)+NC_PPP(opt)+NT_PPP(opt)+NI_PPP(opt)+ND_PPP(opt))
|
||||
#define NB_PPP(opt) (NF_PPP(opt)*MAXSAT)
|
||||
#define NX_PPP(opt) (NR_PPP(opt)+NB_PPP(opt))
|
||||
#define IC_PPP(s,opt) (NP_PPP(opt)+(s))
|
||||
#define IT_PPP(opt) (NP_PPP(opt)+NC_PPP(opt))
|
||||
#define II_PPP(s,opt) (NP_PPP(opt)+NC_PPP(opt)+NT_PPP(opt)+(s)-1)
|
||||
#define ID_PPP(opt) (NP_PPP(opt)+NC_PPP(opt)+NT_PPP(opt)+NI_PPP(opt))
|
||||
#define IB_PPP(s,f,opt) (NR_PPP(opt)+MAXSAT*(f)+(s)-1)
|
||||
|
||||
|
||||
int pppcorr_read(pppcorr_t *corr, const char *file);
|
||||
void pppcorr_free(pppcorr_t *corr);
|
||||
int pppcorr_trop(const pppcorr_t *corr, gtime_t time, const double *pos,
|
||||
double *trp, double *std);
|
||||
int pppcorr_stec(const pppcorr_t *corr, gtime_t time, const double *pos,
|
||||
double *ion, double *std);
|
||||
int ppp_ar(rtk_t *rtk, const obsd_t *obs, int n, int *exc,
|
||||
const nav_t *nav, const double *azel, double *x, double *P);
|
||||
|
||||
double STD(rtk_t *rtk, int i);
|
||||
int pppoutstat(rtk_t *rtk, char *buff);
|
||||
void testeclipse(const obsd_t *obs, int n, const nav_t *nav, double *rs);
|
||||
double yaw_nominal(double beta, double mu);
|
||||
double yaw_angle(int sat, const char *type, int opt, double beta, double mu,
|
||||
double *yaw);
|
||||
int sat_yaw(gtime_t time, int sat, const char *type, int opt,
|
||||
const double *rs, double *exs, double *eys);
|
||||
|
||||
|
||||
int model_phw(gtime_t time, int sat, const char *type, int opt,
|
||||
const double *rs, const double *rr, double *phw);
|
||||
|
||||
double varerr(int sat, int sys, double el, int freq, int type,
|
||||
const prcopt_t *opt);
|
||||
void initx(rtk_t *rtk, double xi, double var, int i);
|
||||
|
||||
double gfmeas(const obsd_t *obs, const nav_t *nav);
|
||||
|
||||
double mwmeas(const obsd_t *obs, const nav_t *nav);
|
||||
|
||||
void corr_meas(const obsd_t *obs, const nav_t *nav, const double *azel,
|
||||
const prcopt_t *opt, const double *dantr,
|
||||
const double *dants, double phw, double *L, double *P,
|
||||
double *Lc, double *Pc);
|
||||
|
||||
|
||||
void detslp_ll(rtk_t *rtk, const obsd_t *obs, int n);
|
||||
|
||||
void detslp_gf(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav);
|
||||
|
||||
void detslp_mw(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav);
|
||||
|
||||
void udpos_ppp(rtk_t *rtk);
|
||||
|
||||
void udclk_ppp(rtk_t *rtk);
|
||||
|
||||
void udtrop_ppp(rtk_t *rtk);
|
||||
|
||||
void udiono_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav);
|
||||
|
||||
void uddcb_ppp(rtk_t *rtk);
|
||||
|
||||
void udbias_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav);
|
||||
|
||||
|
||||
void udstate_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav);
|
||||
|
||||
void satantpcv(const double *rs, const double *rr, const pcv_t *pcv,
|
||||
double *dant);
|
||||
|
||||
double trop_model_prec(gtime_t time, const double *pos,
|
||||
const double *azel, const double *x, double *dtdx,
|
||||
double *var);
|
||||
|
||||
int model_trop(gtime_t time, const double *pos, const double *azel,
|
||||
const prcopt_t *opt, const double *x, double *dtdx,
|
||||
const nav_t *nav, double *dtrp, double *var);
|
||||
|
||||
int model_iono(gtime_t time, const double *pos, const double *azel,
|
||||
const prcopt_t *opt, int sat, const double *x,
|
||||
const nav_t *nav, double *dion, double *var);
|
||||
|
||||
|
||||
int const_corr(const obsd_t *obs, int n, const int *exc,
|
||||
const nav_t *nav, const double *x, const double *pos,
|
||||
const double *azel, rtk_t *rtk, double *v, double *H,
|
||||
double *var);
|
||||
|
||||
int ppp_res(int post, const obsd_t *obs, int n, const double *rs,
|
||||
const double *dts, const double *var_rs, const int *svh,
|
||||
const double *dr, int *exc, const nav_t *nav,
|
||||
const double *x, rtk_t *rtk, double *v, double *H, double *R,
|
||||
double *azel);
|
||||
|
||||
int pppnx(const prcopt_t *opt);
|
||||
|
||||
void update_stat(rtk_t *rtk, const obsd_t *obs, int n, int stat);
|
||||
|
||||
int test_hold_amb(rtk_t *rtk);
|
||||
|
||||
|
||||
void pppos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav);
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
|
|
@ -61,7 +61,7 @@
|
|||
*----------------------------------------------------------------------------*/
|
||||
|
||||
#include "rtklib_preceph.h"
|
||||
|
||||
#include "rtklib_rtkcmn.h"
|
||||
|
||||
/* satellite code to satellite system ----------------------------------------*/
|
||||
int code2sys(char code)
|
||||
|
|
|
|||
|
|
@ -64,7 +64,6 @@
|
|||
#define GNSS_SDR_RTKLIB_PRECEPH_H_
|
||||
|
||||
#include "rtklib.h"
|
||||
#include "rtklib_rtkcmn.h"
|
||||
|
||||
|
||||
const int NMAX = 10; /* order of polynomial interpolation */
|
||||
|
|
|
|||
|
|
@ -50,14 +50,15 @@
|
|||
*
|
||||
*----------------------------------------------------------------------------*/
|
||||
|
||||
#include <stdarg.h>
|
||||
#include <ctype.h>
|
||||
#include "rtklib_rtkcmn.h"
|
||||
//#include <stdarg.h>
|
||||
//#include <ctype.h>
|
||||
#include <dirent.h>
|
||||
#include <time.h>
|
||||
//#include <time.h>
|
||||
#include <sys/time.h>
|
||||
#include <sys/stat.h>
|
||||
#include <sys/types.h>
|
||||
#include "rtklib_rtkcmn.h"
|
||||
|
||||
|
||||
const double gpst0[] = {1980, 1, 6, 0, 0, 0}; /* gps time reference */
|
||||
const double gst0 [] = {1999, 8, 22, 0, 0, 0}; /* galileo system time reference */
|
||||
|
|
@ -3005,13 +3006,15 @@ void freenav(nav_t *nav, int opt)
|
|||
// va_start(ap,format); vfprintf(fp_trace,format,ap); va_end(ap);
|
||||
// fflush(fp_trace);
|
||||
//}
|
||||
//extern void tracemat(int level, const double *A, int n, int m, int p, int q)
|
||||
//{
|
||||
void tracemat(int level, const double *A, int n, int m, int p, int q)
|
||||
{
|
||||
// if (!fp_trace||level>level_trace) return;
|
||||
// matfprint(A,n,m,p,q,fp_trace); fflush(fp_trace);
|
||||
//}
|
||||
//extern void traceobs(int level, const obsd_t *obs, int n)
|
||||
//{
|
||||
}
|
||||
|
||||
|
||||
void traceobs(int level, const obsd_t *obs, int n)
|
||||
{
|
||||
// char str[64],id[16];
|
||||
// int i;
|
||||
//
|
||||
|
|
@ -3025,7 +3028,7 @@ void freenav(nav_t *nav, int opt)
|
|||
// obs[i].code[1],obs[i].SNR[0]*0.25,obs[i].SNR[1]*0.25);
|
||||
// }
|
||||
// fflush(fp_trace);
|
||||
//}
|
||||
}
|
||||
//extern void tracenav(int level, const nav_t *nav)
|
||||
//{
|
||||
// char s1[64],s2[64],id[16];
|
||||
|
|
@ -3434,10 +3437,10 @@ void dops(int ns, const double *azel, double elmin, double *dop)
|
|||
matmul("NT", 4, 4, n, 1.0, H, H, 0.0, Q);
|
||||
if (!matinv(Q, 4))
|
||||
{
|
||||
dop[0] = SQRT(Q[0]+Q[5]+Q[10]+Q[15]); /* GDOP */
|
||||
dop[1] = SQRT(Q[0]+Q[5]+Q[10]); /* PDOP */
|
||||
dop[2] = SQRT(Q[0]+Q[5]); /* HDOP */
|
||||
dop[3] = SQRT(Q[10]); /* VDOP */
|
||||
dop[0] = std::sqrt(Q[0]+Q[5]+Q[10]+Q[15]); /* GDOP */
|
||||
dop[1] = std::sqrt(Q[0]+Q[5]+Q[10]); /* PDOP */
|
||||
dop[2] = std::sqrt(Q[0]+Q[5]); /* HDOP */
|
||||
dop[3] = std::sqrt(Q[10]); /* VDOP */
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -81,6 +81,25 @@
|
|||
#include "rtklib.h"
|
||||
|
||||
|
||||
/* coordinate rotation matrix ------------------------------------------------*/
|
||||
#define Rx(t,X) do { \
|
||||
(X)[0]=1.0; (X)[1]=(X)[2]=(X)[3]=(X)[6]=0.0; \
|
||||
(X)[4]=(X)[8]=cos(t); (X)[7]=sin(t); (X)[5]=-(X)[7]; \
|
||||
} while (0)
|
||||
|
||||
#define Ry(t,X) do { \
|
||||
(X)[4]=1.0; (X)[1]=(X)[3]=(X)[5]=(X)[7]=0.0; \
|
||||
(X)[0]=(X)[8]=cos(t); (X)[2]=sin(t); (X)[6]=-(X)[2]; \
|
||||
} while (0)
|
||||
|
||||
#define Rz(t,X) do { \
|
||||
(X)[8]=1.0; (X)[2]=(X)[5]=(X)[6]=(X)[7]=0.0; \
|
||||
(X)[0]=(X)[4]=cos(t); (X)[3]=sin(t); (X)[1]=-(X)[3]; \
|
||||
} while (0)
|
||||
|
||||
|
||||
|
||||
|
||||
void fatalerr(const char *format, ...);
|
||||
int satno(int sys, int prn);
|
||||
int satsys(int sat, int *prn);
|
||||
|
|
@ -164,21 +183,6 @@ void enu2ecef(const double *pos, const double *e, double *r);
|
|||
void covenu(const double *pos, const double *P, double *Q);
|
||||
void covecef(const double *pos, const double *Q, double *P);
|
||||
|
||||
/* coordinate rotation matrix ------------------------------------------------*/
|
||||
#define Rx(t,X) do { \
|
||||
(X)[0]=1.0; (X)[1]=(X)[2]=(X)[3]=(X)[6]=0.0; \
|
||||
(X)[4]=(X)[8]=cos(t); (X)[7]=sin(t); (X)[5]=-(X)[7]; \
|
||||
} while (0)
|
||||
|
||||
#define Ry(t,X) do { \
|
||||
(X)[4]=1.0; (X)[1]=(X)[3]=(X)[5]=(X)[7]=0.0; \
|
||||
(X)[0]=(X)[8]=cos(t); (X)[2]=sin(t); (X)[6]=-(X)[2]; \
|
||||
} while (0)
|
||||
|
||||
#define Rz(t,X) do { \
|
||||
(X)[8]=1.0; (X)[2]=(X)[5]=(X)[6]=(X)[7]=0.0; \
|
||||
(X)[0]=(X)[4]=cos(t); (X)[3]=sin(t); (X)[1]=-(X)[3]; \
|
||||
} while (0)
|
||||
|
||||
void ast_args(double t, double *f);
|
||||
void nut_iau1980(double t, const double *f, double *dpsi, double *deps);
|
||||
|
|
@ -215,8 +219,8 @@ void freenav(nav_t *nav, int opt);
|
|||
//void tracelevel(int level);
|
||||
void trace (int level, const char *format, ...);
|
||||
//void tracet (int level, const char *format, ...);
|
||||
//void tracemat(int level, const double *A, int n, int m, int p, int q);
|
||||
//void traceobs(int level, const obsd_t *obs, int n);
|
||||
void tracemat(int level, const double *A, int n, int m, int p, int q);
|
||||
void traceobs(int level, const obsd_t *obs, int n);
|
||||
//void tracenav(int level, const nav_t *nav);
|
||||
//void tracegnav(int level, const nav_t *nav);
|
||||
//void tracehnav(int level, const nav_t *nav);
|
||||
|
|
@ -235,7 +239,7 @@ double satwavelen(int sat, int frq, const nav_t *nav);
|
|||
double geodist(const double *rs, const double *rr, double *e);
|
||||
double satazel(const double *pos, const double *e, double *azel);
|
||||
|
||||
#define SQRT(x) ((x)<0.0?0.0:sqrt(x));
|
||||
//#define SQRT(x) ((x)<0.0?0.0:sqrt(x))
|
||||
void dops(int ns, const double *azel, double elmin, double *dop);
|
||||
double ionmodel(gtime_t t, const double *ion, const double *pos,
|
||||
const double *azel);
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -55,50 +55,163 @@
|
|||
#define GNSS_SDR_RTKLIB_RKTPOS_H_
|
||||
|
||||
#include "rtklib.h"
|
||||
#include "rtklib_rtkcmn.h"
|
||||
|
||||
double varerr(const prcopt_t *opt, double el, int sys);
|
||||
double gettgd(int sat, const nav_t *nav);
|
||||
double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
|
||||
int iter, const prcopt_t *opt, double *var);
|
||||
int ionocorr(gtime_t time, const nav_t *nav, int sat, const double *pos,
|
||||
const double *azel, int ionoopt, double *ion, double *var);
|
||||
/* constants/macros ----------------------------------------------------------*/
|
||||
const double VAR_POS = std::pow(30.0, 2.0); /* initial variance of receiver pos (m^2) */
|
||||
const double VAR_VEL = std::pow(10.0, 2.0); /* initial variance of receiver vel ((m/s)^2) */
|
||||
const double VAR_ACC = std::pow(10.0, 2.0); /* initial variance of receiver acc ((m/ss)^2) */
|
||||
const double VAR_HWBIAS = std::pow(1.0, 2.0); /* initial variance of h/w bias ((m/MHz)^2) */
|
||||
const double VAR_GRA = std::pow(0.001, 2.0); /* initial variance of gradient (m^2) */
|
||||
const double INIT_ZWD = 0.15; /* initial zwd (m) */
|
||||
|
||||
int tropcorr(gtime_t time, const nav_t *nav, const double *pos,
|
||||
const double *azel, int tropopt, double *trp, double *var);
|
||||
const double PRN_HWBIA = 1E-6; /* process noise of h/w bias (m/MHz/sqrt(s)) */
|
||||
const double MAXAC = 30.0; /* max accel for doppler slip detection (m/s^2) */
|
||||
|
||||
int rescode(int iter, const obsd_t *obs, int n, const double *rs,
|
||||
const double *dts, const double *vare, const int *svh,
|
||||
const nav_t *nav, const double *x, const prcopt_t *opt,
|
||||
double *v, double *H, double *var, double *azel, int *vsat,
|
||||
double *resp, int *ns);
|
||||
const double VAR_HOLDAMB = 0.001; /* constraint to hold ambiguity (cycle^2) */
|
||||
|
||||
int valsol(const double *azel, const int *vsat, int n,
|
||||
const prcopt_t *opt, const double *v, int nv, int nx,
|
||||
char *msg);
|
||||
const double TTOL_MOVEB = (1.0+2*DTTOL);
|
||||
/* time sync tolerance for moving-baseline (s) */
|
||||
|
||||
int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
|
||||
const double *vare, const int *svh, const nav_t *nav,
|
||||
const prcopt_t *opt, sol_t *sol, double *azel, int *vsat,
|
||||
double *resp, char *msg);
|
||||
/* number of parameters (pos,ionos,tropos,hw-bias,phase-bias,real,estimated) */
|
||||
#define NF_RTK(opt) ((opt)->ionoopt==IONOOPT_IFLC?1:(opt)->nf)
|
||||
#define NP_RTK(opt) ((opt)->dynamics==0?3:9)
|
||||
#define NI_RTK(opt) ((opt)->ionoopt!=IONOOPT_EST?0:MAXSAT)
|
||||
#define NT_RTK(opt) ((opt)->tropopt<TROPOPT_EST?0:((opt)->tropopt<TROPOPT_ESTG?2:6))
|
||||
#define NL_RTK(opt) ((opt)->glomodear!=2?0:NFREQGLO)
|
||||
#define NB_RTK(opt) ((opt)->mode<=PMODE_DGPS?0:MAXSAT*NF_RTK(opt))
|
||||
#define NR_RTK(opt) (NP_RTK(opt)+NI_RTK(opt)+NT_RTK(opt)+NL_RTK(opt))
|
||||
#define NX_RTK(opt) (NR_RTK(opt)+NB_RTK(opt))
|
||||
|
||||
int raim_fde(const obsd_t *obs, int n, const double *rs,
|
||||
const double *dts, const double *vare, const int *svh,
|
||||
const nav_t *nav, const prcopt_t *opt, sol_t *sol,
|
||||
double *azel, int *vsat, double *resp, char *msg);
|
||||
/* state variable index */
|
||||
#define II_RTK(s,opt) (NP_RTK(opt)+(s)-1) /* ionos (s:satellite no) */
|
||||
#define IT_RTK(r,opt) (NP_RTK(opt)+NI_RTK(opt)+NT_RTK(opt)/2*(r)) /* tropos (r:0=rov,1:ref) */
|
||||
#define IL_RTK(f,opt) (NP_RTK(opt)+NI_RTK(opt)+NT_RTK(opt)+(f)) /* receiver h/w bias */
|
||||
#define IB_RTK(s,f,opt) (NR_RTK(opt)+MAXSAT*(f)+(s)-1) /* phase bias (s:satno,f:freq) */
|
||||
|
||||
int resdop(const obsd_t *obs, int n, const double *rs, const double *dts,
|
||||
const nav_t *nav, const double *rr, const double *x,
|
||||
const double *azel, const int *vsat, double *v, double *H);
|
||||
|
||||
void estvel(const obsd_t *obs, int n, const double *rs, const double *dts,
|
||||
const nav_t *nav, const prcopt_t *opt, sol_t *sol,
|
||||
const double *azel, const int *vsat);
|
||||
static int resamb_WLNL(rtk_t *rtk, const obsd_t *obs, const int *sat,
|
||||
const int *iu, const int *ir, int ns, const nav_t *nav,
|
||||
const double *azel) {return 0;}
|
||||
static int resamb_TCAR(rtk_t *rtk, const obsd_t *obs, const int *sat,
|
||||
const int *iu, const int *ir, int ns, const nav_t *nav,
|
||||
const double *azel) {return 0;}
|
||||
|
||||
/* global variables ----------------------------------------------------------*/
|
||||
static int statlevel = 0; /* rtk status output level (0:off) */
|
||||
static FILE *fp_stat = NULL; /* rtk status file pointer */
|
||||
static char file_stat[1024] = ""; /* rtk status file original path */
|
||||
static gtime_t time_stat = {0}; /* rtk status file time */
|
||||
|
||||
|
||||
int rtkopenstat(const char *file, int level);
|
||||
|
||||
void rtkclosestat(void);
|
||||
|
||||
int rtkoutstat(rtk_t *rtk, char *buff);
|
||||
|
||||
void swapsolstat(void);
|
||||
|
||||
void outsolstat(rtk_t *rtk);
|
||||
|
||||
void errmsg(rtk_t *rtk, const char *format, ...);
|
||||
|
||||
double sdobs(const obsd_t *obs, int i, int j, int f);
|
||||
|
||||
double gfobs_L1L2(const obsd_t *obs, int i, int j, const double *lam);
|
||||
|
||||
double gfobs_L1L5(const obsd_t *obs, int i, int j, const double *lam);
|
||||
|
||||
double varerr(int sat, int sys, double el, double bl, double dt, int f,
|
||||
const prcopt_t *opt);
|
||||
|
||||
|
||||
double baseline(const double *ru, const double *rb, double *dr);
|
||||
|
||||
void initx(rtk_t *rtk, double xi, double var, int i);
|
||||
|
||||
int selsat(const obsd_t *obs, double *azel, int nu, int nr,
|
||||
const prcopt_t *opt, int *sat, int *iu, int *ir);
|
||||
|
||||
void udpos(rtk_t *rtk, double tt);
|
||||
|
||||
void udion(rtk_t *rtk, double tt, double bl, const int *sat, int ns);
|
||||
|
||||
void udtrop(rtk_t *rtk, double tt, double bl);
|
||||
|
||||
void udrcvbias(rtk_t *rtk, double tt);
|
||||
|
||||
void detslp_ll(rtk_t *rtk, const obsd_t *obs, int i, int rcv);
|
||||
void detslp_gf_L1L2(rtk_t *rtk, const obsd_t *obs, int i, int j,
|
||||
const nav_t *nav);
|
||||
|
||||
void detslp_gf_L1L5(rtk_t *rtk, const obsd_t *obs, int i, int j,
|
||||
const nav_t *nav);
|
||||
|
||||
void detslp_dop(rtk_t *rtk, const obsd_t *obs, int i, int rcv,
|
||||
const nav_t *nav);
|
||||
|
||||
void udbias(rtk_t *rtk, double tt, const obsd_t *obs, const int *sat,
|
||||
const int *iu, const int *ir, int ns, const nav_t *nav);
|
||||
|
||||
void udstate(rtk_t *rtk, const obsd_t *obs, const int *sat,
|
||||
const int *iu, const int *ir, int ns, const nav_t *nav);
|
||||
|
||||
void zdres_sat(int base, double r, const obsd_t *obs, const nav_t *nav,
|
||||
const double *azel, const double *dant,
|
||||
const prcopt_t *opt, double *y);
|
||||
|
||||
int zdres(int base, const obsd_t *obs, int n, const double *rs,
|
||||
const double *dts, const int *svh, const nav_t *nav,
|
||||
const double *rr, const prcopt_t *opt, int index, double *y,
|
||||
double *e, double *azel);
|
||||
|
||||
int validobs(int i, int j, int f, int nf, double *y);
|
||||
|
||||
void ddcov(const int *nb, int n, const double *Ri, const double *Rj,
|
||||
int nv, double *R);
|
||||
|
||||
int constbl(rtk_t *rtk, const double *x, const double *P, double *v,
|
||||
double *H, double *Ri, double *Rj, int index);
|
||||
|
||||
double prectrop(gtime_t time, const double *pos, int r,
|
||||
const double *azel, const prcopt_t *opt, const double *x,
|
||||
double *dtdx);
|
||||
|
||||
double gloicbcorr(int sat1, int sat2, const prcopt_t *opt, double lam1,
|
||||
double lam2, int f);
|
||||
|
||||
int test_sys(int sys, int m);
|
||||
|
||||
int ddres(rtk_t *rtk, const nav_t *nav, double dt, const double *x,
|
||||
const double *P, const int *sat, double *y, double *e,
|
||||
double *azel, const int *iu, const int *ir, int ns, double *v,
|
||||
double *H, double *R, int *vflg);
|
||||
|
||||
double intpres(gtime_t time, const obsd_t *obs, int n, const nav_t *nav,
|
||||
rtk_t *rtk, double *y);
|
||||
|
||||
|
||||
int ddmat(rtk_t *rtk, double *D);
|
||||
|
||||
void restamb(rtk_t *rtk, const double *bias, int nb, double *xa);
|
||||
|
||||
void holdamb(rtk_t *rtk, const double *xa);
|
||||
|
||||
int resamb_LAMBDA(rtk_t *rtk, double *bias, double *xa);
|
||||
|
||||
int valpos(rtk_t *rtk, const double *v, const double *R, const int *vflg,
|
||||
int nv, double thres);
|
||||
|
||||
int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr,
|
||||
const nav_t *nav);
|
||||
|
||||
void rtkinit(rtk_t *rtk, const prcopt_t *opt);
|
||||
|
||||
void rtkfree(rtk_t *rtk);
|
||||
|
||||
int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav);
|
||||
|
||||
int pntpos(const obsd_t *obs, int n, const nav_t *nav,
|
||||
const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat,
|
||||
char *msg);
|
||||
|
||||
int lexioncorr(gtime_t time, const nav_t *nav, const double *pos,
|
||||
const double *azel, double *delay, double *var);
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -60,7 +60,7 @@
|
|||
*----------------------------------------------------------------------------*/
|
||||
|
||||
#include "rtklib_sbas.h"
|
||||
|
||||
#include "rtklib_rtkcmn.h"
|
||||
|
||||
/* extract field from line ---------------------------------------------------*/
|
||||
char *getfield(char *p, int pos)
|
||||
|
|
|
|||
|
|
@ -63,10 +63,10 @@
|
|||
#define GNSS_SDR_RTKLIB_SBAS_H_
|
||||
|
||||
#include "rtklib.h"
|
||||
#include "rtklib_rtkcmn.h"
|
||||
|
||||
/* constants -----------------------------------------------------------------*/
|
||||
|
||||
#define WEEKOFFSET 1024 /* gps week offset for NovAtel OEM-3 */
|
||||
const int WEEKOFFSET = 1024; /* gps week offset for NovAtel OEM-3 */
|
||||
|
||||
/* sbas igp definition -------------------------------------------------------*/
|
||||
static const short
|
||||
|
|
|
|||
|
|
@ -0,0 +1,321 @@
|
|||
/*!
|
||||
* \file rtklib_tides.cc
|
||||
* \brief Tidal displacement corrections
|
||||
* \authors <ul>
|
||||
* <li> 2007-2008, T. Takasu
|
||||
* <li> 2017, Javier Arribas
|
||||
* <li> 2017, Carles Fernandez
|
||||
* </ul>
|
||||
*
|
||||
* This is a derived work from RTKLIB http://www.rtklib.com/
|
||||
* The original source code at https://github.com/tomojitakasu/RTKLIB is
|
||||
* released under the BSD 2-clause license with an additional exclusive clause
|
||||
* that does not apply here. This additional clause is reproduced below:
|
||||
*
|
||||
* " The software package includes some companion executive binaries or shared
|
||||
* libraries necessary to execute APs on Windows. These licenses succeed to the
|
||||
* original ones of these software. "
|
||||
*
|
||||
* Neither the executive binaries nor the shared libraries are required by, used
|
||||
* or included in GNSS-SDR.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
* Copyright (C) 2007-2008, T. Takasu
|
||||
* Copyright (C) 2017, Javier Arribas
|
||||
* Copyright (C) 2017, Carles Fernandez
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are
|
||||
* met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
|
||||
* A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
|
||||
* HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
|
||||
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
|
||||
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
|
||||
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
|
||||
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
*----------------------------------------------------------------------------*/
|
||||
|
||||
#include "rtklib_tides.h"
|
||||
#include "rtklib_rtkcmn.h"
|
||||
|
||||
|
||||
|
||||
/* solar/lunar tides (ref [2] 7) ---------------------------------------------*/
|
||||
//#ifndef IERS_MODEL
|
||||
void tide_pl(const double *eu, const double *rp, double GMp,
|
||||
const double *pos, double *dr)
|
||||
{
|
||||
const double H3=0.292,L3=0.015;
|
||||
double r,ep[3],latp,lonp,p,K2,K3,a,H2,L2,dp,du,cosp,sinl,cosl;
|
||||
int i;
|
||||
|
||||
trace(4,"tide_pl : pos=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D);
|
||||
|
||||
if ((r=norm(rp,3))<=0.0) return;
|
||||
|
||||
for (i=0;i<3;i++) ep[i]=rp[i]/r;
|
||||
|
||||
K2=GMp/GME*std::pow(RE_WGS84, 2.04)*std::pow(RE_WGS84, 2.0)/(r*r*r);
|
||||
K3=K2*RE_WGS84/r;
|
||||
latp=asin(ep[2]); lonp=atan2(ep[1],ep[0]);
|
||||
cosp=cos(latp); sinl=sin(pos[0]); cosl=cos(pos[0]);
|
||||
|
||||
/* step1 in phase (degree 2) */
|
||||
p=(3.0*sinl*sinl-1.0)/2.0;
|
||||
H2=0.6078-0.0006*p;
|
||||
L2=0.0847+0.0002*p;
|
||||
a=dot(ep,eu,3);
|
||||
dp=K2*3.0*L2*a;
|
||||
du=K2*(H2*(1.5*a*a-0.5)-3.0*L2*a*a);
|
||||
|
||||
/* step1 in phase (degree 3) */
|
||||
dp+=K3*L3*(7.5*a*a-1.5);
|
||||
du+=K3*(H3*(2.5*a*a*a-1.5*a)-L3*(7.5*a*a-1.5)*a);
|
||||
|
||||
/* step1 out-of-phase (only radial) */
|
||||
du+=3.0/4.0*0.0025*K2*sin(2.0*latp)*sin(2.0*pos[0])*sin(pos[1]-lonp);
|
||||
du+=3.0/4.0*0.0022*K2*cosp*cosp*cosl*cosl*sin(2.0*(pos[1]-lonp));
|
||||
|
||||
dr[0]=dp*ep[0]+du*eu[0];
|
||||
dr[1]=dp*ep[1]+du*eu[1];
|
||||
dr[2]=dp*ep[2]+du*eu[2];
|
||||
|
||||
trace(5,"tide_pl : dr=%.3f %.3f %.3f\n",dr[0],dr[1],dr[2]);
|
||||
}
|
||||
|
||||
|
||||
/* displacement by solid earth tide (ref [2] 7) ------------------------------*/
|
||||
void tide_solid(const double *rsun, const double *rmoon,
|
||||
const double *pos, const double *E, double gmst, int opt,
|
||||
double *dr)
|
||||
{
|
||||
double dr1[3],dr2[3],eu[3],du,dn,sinl,sin2l;
|
||||
|
||||
trace(3,"tide_solid: pos=%.3f %.3f opt=%d\n",pos[0]*R2D,pos[1]*R2D,opt);
|
||||
|
||||
/* step1: time domain */
|
||||
eu[0]=E[2]; eu[1]=E[5]; eu[2]=E[8];
|
||||
tide_pl(eu,rsun, GMS,pos,dr1);
|
||||
tide_pl(eu,rmoon,GMM,pos,dr2);
|
||||
|
||||
/* step2: frequency domain, only K1 radial */
|
||||
sin2l=sin(2.0*pos[0]);
|
||||
du=-0.012*sin2l*sin(gmst+pos[1]);
|
||||
|
||||
dr[0]=dr1[0]+dr2[0]+du*E[2];
|
||||
dr[1]=dr1[1]+dr2[1]+du*E[5];
|
||||
dr[2]=dr1[2]+dr2[2]+du*E[8];
|
||||
|
||||
/* eliminate permanent deformation */
|
||||
if (opt&8)
|
||||
{
|
||||
sinl=sin(pos[0]);
|
||||
du=0.1196*(1.5*sinl*sinl-0.5);
|
||||
dn=0.0247*sin2l;
|
||||
dr[0]+=du*E[2]+dn*E[1];
|
||||
dr[1]+=du*E[5]+dn*E[4];
|
||||
dr[2]+=du*E[8]+dn*E[7];
|
||||
}
|
||||
trace(5,"tide_solid: dr=%.3f %.3f %.3f\n",dr[0],dr[1],dr[2]);
|
||||
}
|
||||
//#endif /* !IERS_MODEL */
|
||||
|
||||
|
||||
/* displacement by ocean tide loading (ref [2] 7) ----------------------------*/
|
||||
void tide_oload(gtime_t tut, const double *odisp, double *denu)
|
||||
{
|
||||
const double args[][5]={
|
||||
{1.40519E-4, 2.0,-2.0, 0.0, 0.00}, /* M2 */
|
||||
{1.45444E-4, 0.0, 0.0, 0.0, 0.00}, /* S2 */
|
||||
{1.37880E-4, 2.0,-3.0, 1.0, 0.00}, /* N2 */
|
||||
{1.45842E-4, 2.0, 0.0, 0.0, 0.00}, /* K2 */
|
||||
{0.72921E-4, 1.0, 0.0, 0.0, 0.25}, /* K1 */
|
||||
{0.67598E-4, 1.0,-2.0, 0.0,-0.25}, /* O1 */
|
||||
{0.72523E-4,-1.0, 0.0, 0.0,-0.25}, /* P1 */
|
||||
{0.64959E-4, 1.0,-3.0, 1.0,-0.25}, /* Q1 */
|
||||
{0.53234E-5, 0.0, 2.0, 0.0, 0.00}, /* Mf */
|
||||
{0.26392E-5, 0.0, 1.0,-1.0, 0.00}, /* Mm */
|
||||
{0.03982E-5, 2.0, 0.0, 0.0, 0.00} /* Ssa */
|
||||
};
|
||||
const double ep1975[]={1975,1,1,0,0,0};
|
||||
double ep[6],fday,days,t,t2,t3,a[5],ang,dp[3]={0};
|
||||
int i,j;
|
||||
|
||||
trace(3,"tide_oload:\n");
|
||||
|
||||
/* angular argument: see subroutine arg.f for reference [1] */
|
||||
time2epoch(tut,ep);
|
||||
fday=ep[3]*3600.0+ep[4]*60.0+ep[5];
|
||||
ep[3]=ep[4]=ep[5]=0.0;
|
||||
days=timediff(epoch2time(ep),epoch2time(ep1975))/86400.0+1.0;
|
||||
t=(27392.500528+1.000000035*days)/36525.0;
|
||||
t2=t*t; t3=t2*t;
|
||||
|
||||
a[0]=fday;
|
||||
a[1]=(279.69668+36000.768930485*t+3.03E-4*t2)*D2R; /* H0 */
|
||||
a[2]=(270.434358+481267.88314137*t-0.001133*t2+1.9E-6*t3)*D2R; /* S0 */
|
||||
a[3]=(334.329653+4069.0340329577*t-0.010325*t2-1.2E-5*t3)*D2R; /* P0 */
|
||||
a[4]=2.0*PI;
|
||||
|
||||
/* displacements by 11 constituents */
|
||||
for (i=0;i<11;i++)
|
||||
{
|
||||
ang=0.0;
|
||||
for (j=0;j<5;j++) ang+=a[j]*args[i][j];
|
||||
for (j=0;j<3;j++) dp[j]+=odisp[j+i*6]*cos(ang-odisp[j+3+i*6]*D2R);
|
||||
}
|
||||
denu[0]=-dp[1];
|
||||
denu[1]=-dp[2];
|
||||
denu[2]= dp[0];
|
||||
|
||||
trace(5,"tide_oload: denu=%.3f %.3f %.3f\n",denu[0],denu[1],denu[2]);
|
||||
}
|
||||
|
||||
|
||||
/* iers mean pole (ref [7] eq.7.25) ------------------------------------------*/
|
||||
void iers_mean_pole(gtime_t tut, double *xp_bar, double *yp_bar)
|
||||
{
|
||||
const double ep2000[]={2000,1,1,0,0,0};
|
||||
double y,y2,y3;
|
||||
|
||||
y=timediff(tut,epoch2time(ep2000))/86400.0/365.25;
|
||||
|
||||
if (y<3653.0/365.25)
|
||||
{ /* until 2010.0 */
|
||||
y2=y*y; y3=y2*y;
|
||||
*xp_bar= 55.974+1.8243*y+0.18413*y2+0.007024*y3; /* (mas) */
|
||||
*yp_bar=346.346+1.7896*y-0.10729*y2-0.000908*y3;
|
||||
}
|
||||
else
|
||||
{ /* after 2010.0 */
|
||||
*xp_bar= 23.513+7.6141*y; /* (mas) */
|
||||
*yp_bar=358.891-0.6287*y;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/* displacement by pole tide (ref [7] eq.7.26) --------------------------------*/
|
||||
void tide_pole(gtime_t tut, const double *pos, const double *erpv,
|
||||
double *denu)
|
||||
{
|
||||
double xp_bar,yp_bar,m1,m2,cosl,sinl;
|
||||
|
||||
trace(3,"tide_pole: pos=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D);
|
||||
|
||||
/* iers mean pole (mas) */
|
||||
iers_mean_pole(tut,&xp_bar,&yp_bar);
|
||||
|
||||
/* ref [7] eq.7.24 */
|
||||
m1= erpv[0]/AS2R-xp_bar*1E-3; /* (as) */
|
||||
m2=-erpv[1]/AS2R+yp_bar*1E-3;
|
||||
|
||||
/* sin(2*theta) = sin(2*phi), cos(2*theta)=-cos(2*phi) */
|
||||
cosl=cos(pos[1]);
|
||||
sinl=sin(pos[1]);
|
||||
denu[0]= 9E-3*sin(pos[0]) *(m1*sinl-m2*cosl); /* de= Slambda (m) */
|
||||
denu[1]= -9E-3*cos(2.0*pos[0])*(m1*cosl+m2*sinl); /* dn=-Stheta (m) */
|
||||
denu[2]=-33E-3*sin(2.0*pos[0])*(m1*cosl+m2*sinl); /* du= Sr (m) */
|
||||
|
||||
trace(5,"tide_pole : denu=%.3f %.3f %.3f\n",denu[0],denu[1],denu[2]);
|
||||
}
|
||||
|
||||
|
||||
/* tidal displacement ----------------------------------------------------------
|
||||
* displacements by earth tides
|
||||
* args : gtime_t tutc I time in utc
|
||||
* double *rr I site position (ecef) (m)
|
||||
* int opt I options (or of the followings)
|
||||
* 1: solid earth tide
|
||||
* 2: ocean tide loading
|
||||
* 4: pole tide
|
||||
* 8: elimate permanent deformation
|
||||
* double *erp I earth rotation parameters (NULL: not used)
|
||||
* double *odisp I ocean loading parameters (NULL: not used)
|
||||
* odisp[0+i*6]: consituent i amplitude radial(m)
|
||||
* odisp[1+i*6]: consituent i amplitude west (m)
|
||||
* odisp[2+i*6]: consituent i amplitude south (m)
|
||||
* odisp[3+i*6]: consituent i phase radial (deg)
|
||||
* odisp[4+i*6]: consituent i phase west (deg)
|
||||
* odisp[5+i*6]: consituent i phase south (deg)
|
||||
* (i=0:M2,1:S2,2:N2,3:K2,4:K1,5:O1,6:P1,7:Q1,
|
||||
* 8:Mf,9:Mm,10:Ssa)
|
||||
* double *dr O displacement by earth tides (ecef) (m)
|
||||
* return : none
|
||||
* notes : see ref [1], [2] chap 7
|
||||
* see ref [4] 5.2.1, 5.2.2, 5.2.3
|
||||
* ver.2.4.0 does not use ocean loading and pole tide corrections
|
||||
*-----------------------------------------------------------------------------*/
|
||||
void tidedisp(gtime_t tutc, const double *rr, int opt, const erp_t *erp,
|
||||
const double *odisp, double *dr)
|
||||
{
|
||||
gtime_t tut;
|
||||
double pos[2],E[9],drt[3],denu[3],rs[3],rm[3],gmst,erpv[5]={0};
|
||||
int i;
|
||||
#ifdef IERS_MODEL
|
||||
double ep[6],fhr;
|
||||
int year,mon,day;
|
||||
#endif
|
||||
|
||||
trace(3,"tidedisp: tutc=%s\n",time_str(tutc,0));
|
||||
|
||||
if (erp) geterp(erp,tutc,erpv);
|
||||
|
||||
tut=timeadd(tutc,erpv[2]);
|
||||
|
||||
dr[0]=dr[1]=dr[2]=0.0;
|
||||
|
||||
if (norm(rr,3)<=0.0) return;
|
||||
|
||||
pos[0]=asin(rr[2]/norm(rr,3));
|
||||
pos[1]=atan2(rr[1],rr[0]);
|
||||
xyz2enu(pos,E);
|
||||
|
||||
if (opt&1)
|
||||
{ /* solid earth tides */
|
||||
|
||||
/* sun and moon position in ecef */
|
||||
sunmoonpos(tutc,erpv,rs,rm,&gmst);
|
||||
|
||||
#ifdef IERS_MODEL
|
||||
time2epoch(tutc,ep);
|
||||
year=(int)ep[0];
|
||||
mon =(int)ep[1];
|
||||
day =(int)ep[2];
|
||||
fhr =ep[3]+ep[4]/60.0+ep[5]/3600.0;
|
||||
|
||||
/* call DEHANTTIDEINEL */
|
||||
// dehanttideinel_((double *)rr,&year,&mon,&day,&fhr,rs,rm,drt);
|
||||
#else
|
||||
tide_solid(rs,rm,pos,E,gmst,opt,drt);
|
||||
#endif
|
||||
for (i=0;i<3;i++) dr[i]+=drt[i];
|
||||
}
|
||||
if ((opt&2)&&odisp)
|
||||
{ /* ocean tide loading */
|
||||
tide_oload(tut,odisp,denu);
|
||||
matmul("TN",3,1,3,1.0,E,denu,0.0,drt);
|
||||
for (i=0;i<3;i++) dr[i]+=drt[i];
|
||||
}
|
||||
if ((opt&4)&&erp)
|
||||
{ /* pole tide */
|
||||
tide_pole(tut,pos,erpv,denu);
|
||||
matmul("TN",3,1,3,1.0,E,denu,0.0,drt);
|
||||
for (i=0;i<3;i++) dr[i]+=drt[i];
|
||||
}
|
||||
trace(5,"tidedisp: dr=%.3f %.3f %.3f\n",dr[0],dr[1],dr[2]);
|
||||
}
|
||||
|
|
@ -0,0 +1,92 @@
|
|||
/*!
|
||||
* \file rtklib_tides.h
|
||||
* \brief Tidal displacement corrections
|
||||
* \authors <ul>
|
||||
* <li> 2015, T. Takasu
|
||||
* <li> 2017, Javier Arribas
|
||||
* <li> 2017, Carles Fernandez
|
||||
* </ul>
|
||||
*
|
||||
* This is a derived work from RTKLIB http://www.rtklib.com/
|
||||
* The original source code at https://github.com/tomojitakasu/RTKLIB is
|
||||
* released under the BSD 2-clause license with an additional exclusive clause
|
||||
* that does not apply here. This additional clause is reproduced below:
|
||||
*
|
||||
* " The software package includes some companion executive binaries or shared
|
||||
* libraries necessary to execute APs on Windows. These licenses succeed to the
|
||||
* original ones of these software. "
|
||||
*
|
||||
* Neither the executive binaries nor the shared libraries are required by, used
|
||||
* or included in GNSS-SDR.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
* Copyright (C) 2015, T. Takasu
|
||||
* Copyright (C) 2017, Javier Arribas
|
||||
* Copyright (C) 2017, Carles Fernandez
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are
|
||||
* met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
|
||||
* A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
|
||||
* HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
|
||||
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
|
||||
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
|
||||
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
|
||||
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* References:
|
||||
* [1] D.D.McCarthy, IERS Technical Note 21, IERS Conventions 1996, July 1996
|
||||
* [2] D.D.McCarthy and G.Petit, IERS Technical Note 32, IERS Conventions
|
||||
* 2003, November 2003
|
||||
* [3] D.A.Vallado, Fundamentals of Astrodynamics and Applications 2nd ed,
|
||||
* Space Technology Library, 2004
|
||||
* [4] J.Kouba, A Guide to using International GNSS Service (IGS) products,
|
||||
* May 2009
|
||||
* [5] G.Petit and B.Luzum (eds), IERS Technical Note No. 36, IERS
|
||||
* Conventions (2010), 2010
|
||||
*----------------------------------------------------------------------------*/
|
||||
|
||||
|
||||
#ifndef GNSS_SDR_RTKLIB_TIDES_H_
|
||||
#define GNSS_SDR_RTKLIB_TIDES_H_
|
||||
|
||||
|
||||
#include "rtklib.h"
|
||||
|
||||
|
||||
const double GME = 3.986004415E+14; /* earth gravitational constant */
|
||||
const double GMS = 1.327124E+20; /* sun gravitational constant */
|
||||
const double GMM = 4.902801E+12; /* moon gravitational constant */
|
||||
|
||||
void tide_pl(const double *eu, const double *rp, double GMp,
|
||||
const double *pos, double *dr);
|
||||
|
||||
void tide_solid(const double *rsun, const double *rmoon,
|
||||
const double *pos, const double *E, double gmst, int opt,
|
||||
double *dr);
|
||||
|
||||
void tide_oload(gtime_t tut, const double *odisp, double *denu);
|
||||
|
||||
void iers_mean_pole(gtime_t tut, double *xp_bar, double *yp_bar);
|
||||
|
||||
|
||||
void tide_pole(gtime_t tut, const double *pos, const double *erpv,
|
||||
double *denu);
|
||||
|
||||
void tidedisp(gtime_t tutc, const double *rr, int opt, const erp_t *erp,
|
||||
const double *odisp, double *dr);
|
||||
#endif
|
||||
Loading…
Reference in New Issue