diff --git a/src/algorithms/libs/rtklib/CMakeLists.txt b/src/algorithms/libs/rtklib/CMakeLists.txt index 3f2f907f6..9392faa01 100644 --- a/src/algorithms/libs/rtklib/CMakeLists.txt +++ b/src/algorithms/libs/rtklib/CMakeLists.txt @@ -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 ) diff --git a/src/algorithms/libs/rtklib/rtklib.h b/src/algorithms/libs/rtklib/rtklib.h index 7a68e50b5..685c76ebc 100644 --- a/src/algorithms/libs/rtklib/rtklib.h +++ b/src/algorithms/libs/rtklib/rtklib.h @@ -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 diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.cc b/src/algorithms/libs/rtklib/rtklib_conversions.cc index 1937a9998..dc32ddcc1 100644 --- a/src/algorithms/libs/rtklib/rtklib_conversions.cc +++ b/src/algorithms/libs/rtklib/rtklib_conversions.cc @@ -1,57 +1,35 @@ /*! * \file rtklib_conversions.cc * \brief GNSS-SDR to RTKLIB data structures conversion functions - * \authors - * - * 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 . + * + * ------------------------------------------------------------------------- + */ #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) { diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.h b/src/algorithms/libs/rtklib/rtklib_conversions.h index cfbb67958..71d3fa3bd 100644 --- a/src/algorithms/libs/rtklib/rtklib_conversions.h +++ b/src/algorithms/libs/rtklib/rtklib_conversions.h @@ -1,60 +1,37 @@ /*! * \file rtklib_conversions.h * \brief GNSS-SDR to RTKLIB data structures conversion functions - * \authors - * - * 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 . + * + * ------------------------------------------------------------------------- + */ #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" diff --git a/src/algorithms/libs/rtklib/rtklib_ephemeris.cc b/src/algorithms/libs/rtklib/rtklib_ephemeris.cc index 26da3195e..ad2825f1b 100644 --- a/src/algorithms/libs/rtklib/rtklib_ephemeris.cc +++ b/src/algorithms/libs/rtklib/rtklib_ephemeris.cc @@ -51,7 +51,9 @@ *----------------------------------------------------------------------------*/ #include "rtklib_ephemeris.h" - +#include "rtklib_rtkcmn.h" +#include "rtklib_sbas.h" +#include "rtklib_preceph.h" /* constants ------------------------------------------------------*/ diff --git a/src/algorithms/libs/rtklib/rtklib_ephemeris.h b/src/algorithms/libs/rtklib/rtklib_ephemeris.h index 8275d4b94..1ccc32f4a 100644 --- a/src/algorithms/libs/rtklib/rtklib_ephemeris.h +++ b/src/algorithms/libs/rtklib/rtklib_ephemeris.h @@ -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); diff --git a/src/algorithms/libs/rtklib/rtklib_ionex.cc b/src/algorithms/libs/rtklib/rtklib_ionex.cc index 2364f0531..bdfd0d06e 100644 --- a/src/algorithms/libs/rtklib/rtklib_ionex.cc +++ b/src/algorithms/libs/rtklib/rtklib_ionex.cc @@ -58,7 +58,7 @@ *----------------------------------------------------------------------------*/ #include "rtklib_ionex.h" - +#include "rtklib_rtkcmn.h" /* get index -----------------------------------------------------------------*/ int getindex(double value, const double *range) diff --git a/src/algorithms/libs/rtklib/rtklib_ionex.h b/src/algorithms/libs/rtklib/rtklib_ionex.h index a3cb71c84..7ed73e3ee 100644 --- a/src/algorithms/libs/rtklib/rtklib_ionex.h +++ b/src/algorithms/libs/rtklib/rtklib_ionex.h @@ -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) */ diff --git a/src/algorithms/libs/rtklib/rtklib_lambda.cc b/src/algorithms/libs/rtklib/rtklib_lambda.cc new file mode 100644 index 000000000..32082ac97 --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_lambda.cc @@ -0,0 +1,327 @@ +/*! + * \file rtklib_lambda.cc + * \brief Integer ambiguity resolution + * \authors + * + * 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= 0) + { + if (j<=k) for (i = j+1; is[imax]) imax = nn; + for (i = 0; i= 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 + *
  • 2007-2008, T. Takasu + *
  • 2017, Javier Arribas + *
  • 2017, Carles Fernandez + * + * + * 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 diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index a60fbdbec..35f173cd9 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -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) diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.h b/src/algorithms/libs/rtklib/rtklib_pntpos.h index f44ac9809..5dd969943 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.h +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.h @@ -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); diff --git a/src/algorithms/libs/rtklib/rtklib_ppp.cc b/src/algorithms/libs/rtklib/rtklib_ppp.cc new file mode 100644 index 000000000..678a9b8ea --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_ppp.cc @@ -0,0 +1,1352 @@ +/*! + * \file rtklib_ppp.cc + * \brief Precise Point Positioning + * \authors
      + *
    • 2007-2008, T. Takasu + *
    • 2017, Javier Arribas + *
    • 2017, Carles Fernandez + *
    + * + * 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_ppp.h" +#include "rtklib_rtkcmn.h" +#include "rtklib_sbas.h" +#include "rtklib_ephemeris.h" +#include "rtklib_ionex.h" +#include "rtklib_tides.h" + +/* read ppp corrections -------------------------------------------------------- + * read ppp correction data from external file + * args : pppcorr_t *corr IO ppp correction data + * char *file I file + * return : status (1:ok,0:error) + * notes : file types are recognized by file extenstions as follows. + * .stat,.STAT : solution status file by rtklib + * .stec,.STEC : stec parameters file by mgest + * others : sinex troposphere file + * read data are added to ppp correction data. + * To clear data, call pppcorr_free() + *-----------------------------------------------------------------------------*/ +int pppcorr_read(pppcorr_t *corr, const char *file) +{ + return 0; +} + + +/* free ppp corrections -------------------------------------------------------- + * free and clear ppp correction data + * args : pppcorr_t *corr IO ppp correction data + * return : none + *-----------------------------------------------------------------------------*/ +void pppcorr_free(pppcorr_t *corr) +{ +} + + +/* get tropospheric correction ------------------------------------------------- + * get tropospheric correction from ppp correcion data + * args : pppcorr_t *corr I ppp correction data + * gtime_t time I time (GPST) + * double *pos I receiver position {lat,lon,heght} (rad,m) + * double *trp O tropos parameters {ztd,grade,gradn} (m) + * double *std O standard deviation (m) + * return : status (1:ok,0:error) + *-----------------------------------------------------------------------------*/ +int pppcorr_trop(const pppcorr_t *corr, gtime_t time, const double *pos, + double *trp, double *std) +{ + return 0; +} + + +int pppcorr_stec(const pppcorr_t *corr, gtime_t time, const double *pos, + double *ion, double *std) +{ + return 0; +} + + +/* ambiguity resolution in ppp -----------------------------------------------*/ +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) +{ + return 0; +} + + +/* standard deviation of state -----------------------------------------------*/ +double STD(rtk_t *rtk, int i) +{ + if (rtk->sol.stat==SOLQ_FIX) return std::sqrt(rtk->Pa[i+i*rtk->nx]); + return std::sqrt(rtk->P[i+i*rtk->nx]); +} + + +/* write solution status for PPP ---------------------------------------------*/ +int pppoutstat(rtk_t *rtk, char *buff) +{ + ssat_t *ssat; + double tow,pos[3],vel[3],acc[3],*x; + int i,j,week; + char id[32],*p=buff; + + if (!rtk->sol.stat) return 0; + + trace(3,"pppoutstat:\n"); + + tow=time2gpst(rtk->sol.time,&week); + + x=rtk->sol.stat==SOLQ_FIX?rtk->xa:rtk->x; + + /* receiver position */ + p+=sprintf(p,"$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n",week,tow, + rtk->sol.stat,x[0],x[1],x[2],STD(rtk,0),STD(rtk,1),STD(rtk,2)); + + /* receiver velocity and acceleration */ + if (rtk->opt.dynamics) + { + ecef2pos(rtk->sol.rr,pos); + ecef2enu(pos,rtk->x+3,vel); + ecef2enu(pos,rtk->x+6,acc); + p+=sprintf(p,"$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f," + "%.4f,%.5f,%.5f,%.5f\n",week,tow,rtk->sol.stat,vel[0],vel[1], + vel[2],acc[0],acc[1],acc[2],0.0,0.0,0.0,0.0,0.0,0.0); + } + /* receiver clocks */ + i=IC_PPP(0,&rtk->opt); + p+=sprintf(p,"$CLK,%d,%.3f,%d,%d,%.3f,%.3f,%.3f,%.3f\n", + week,tow,rtk->sol.stat,1,x[i]*1E9/SPEED_OF_LIGHT,x[i+1]*1E9/SPEED_OF_LIGHT, + STD(rtk,i)*1E9/SPEED_OF_LIGHT,STD(rtk,i+1)*1E9/SPEED_OF_LIGHT); + + /* tropospheric parameters */ + if (rtk->opt.tropopt==TROPOPT_EST||rtk->opt.tropopt==TROPOPT_ESTG) + { + i=IT_PPP(&rtk->opt); + p+=sprintf(p,"$TROP,%d,%.3f,%d,%d,%.4f,%.4f\n",week,tow,rtk->sol.stat, + 1,x[i],STD(rtk,i)); + } + if (rtk->opt.tropopt==TROPOPT_ESTG) + { + i=IT_PPP(&rtk->opt); + p+=sprintf(p,"$TRPG,%d,%.3f,%d,%d,%.5f,%.5f,%.5f,%.5f\n",week,tow, + rtk->sol.stat,1,x[i+1],x[i+2],STD(rtk,i+1),STD(rtk,i+2)); + } + /* ionosphere parameters */ + if (rtk->opt.ionoopt==IONOOPT_EST) + { + for (i=0;issat+i; + if (!ssat->vs) continue; + j=II_PPP(i+1,&rtk->opt); + if (rtk->x[j]==0.0) continue; + satno2id(i+1,id); + p+=sprintf(p,"$ION,%d,%.3f,%d,%s,%.1f,%.1f,%.4f,%.4f\n",week,tow, + rtk->sol.stat,id,rtk->ssat[i].azel[0]*R2D, + rtk->ssat[i].azel[1]*R2D,x[j],STD(rtk,j)); + } + } +#ifdef OUTSTAT_AMB + /* ambiguity parameters */ + for (i=0;iopt);j++) + { + k=IB_PPP(i+1,j,&rtk->opt); + if (rtk->x[k]==0.0) continue; + satno2id(i+1,id); + p+=sprintf(p,"$AMB,%d,%.3f,%d,%s,%d,%.4f,%.4f\n",week,tow, + rtk->sol.stat,id,j+1,x[k],STD(rtk,k)); + } +#endif + return (int)(p-buff); +} + + +/* exclude meas of eclipsing satellite (block IIA) ---------------------------*/ +void testeclipse(const obsd_t *obs, int n, const nav_t *nav, double *rs) +{ + double rsun[3],esun[3],r,ang,erpv[5]={0},cosa; + int i,j; + const char *type; + + trace(3,"testeclipse:\n"); + + /* unit vector of sun direction (ecef) */ + sunmoonpos(gpst2utc(obs[0].time),erpv,rsun,NULL,NULL); + normv3(rsun,esun); + + for (i=0;ipcvs[obs[i].sat-1].type; + + if ((r=norm(rs+i*6,3))<=0.0) continue; + + /* only block IIA */ + if (*type&&!strstr(type,"BLOCK IIA")) continue; + + /* sun-earth-satellite angle */ + cosa=dot(rs+i*6,esun,3)/r; + cosa=cosa<-1.0?-1.0:(cosa>1.0?1.0:cosa); + ang=acos(cosa); + + /* test eclipse */ + if (angRE_WGS84) continue; + + trace(3,"eclipsing sat excluded %s sat=%2d\n",time_str(obs[0].time,0), + obs[i].sat); + + for (j=0;j<3;j++) rs[j+i*6]=0.0; + } +} + + +/* nominal yaw-angle ---------------------------------------------------------*/ +double yaw_nominal(double beta, double mu) +{ + if (fabs(beta)<1E-12&&fabs(mu)<1E-12) return PI; + return atan2(-tan(beta),sin(mu))+PI; +} + + +/* yaw-angle of satellite ----------------------------------------------------*/ +double yaw_angle(int sat, const char *type, int opt, double beta, double mu, + double *yaw) +{ + *yaw=yaw_nominal(beta,mu); + return 1; +} + + +/* satellite attitude model --------------------------------------------------*/ +int sat_yaw(gtime_t time, int sat, const char *type, int opt, + const double *rs, double *exs, double *eys) +{ + double rsun[3],ri[6],es[3],esun[3],n[3],p[3],en[3],ep[3],ex[3],E,beta,mu; + double yaw,cosy,siny,erpv[5]={0}; + int i; + + sunmoonpos(gpst2utc(time),erpv,rsun,NULL,NULL); + + /* beta and orbit angle */ + matcpy(ri,rs,6,1); + ri[3]-=DEFAULT_OMEGA_EARTH_DOT*ri[1]; + ri[4]+=DEFAULT_OMEGA_EARTH_DOT*ri[0]; + cross3(ri,ri+3,n); + cross3(rsun,n,p); + if (!normv3(rs,es)||!normv3(rsun,esun)||!normv3(n,en)|| + !normv3(p,ep)) return 0; + beta=PI/2.0-acos(dot(esun,en,3)); + E=acos(dot(es,ep,3)); + mu=PI/2.0+(dot(es,esun,3)<=0?-E:E); + if (mu<-PI/2.0) mu+=2.0*PI; + else if (mu>=PI/2.0) mu-=2.0*PI; + + /* yaw-angle of satellite */ + if (!yaw_angle(sat,type,opt,beta,mu,&yaw)) return 0; + + /* satellite fixed x,y-vector */ + cross3(en,es,ex); + cosy=cos(yaw); + siny=sin(yaw); + for (i=0;i<3;i++) + { + exs[i]=-siny*en[i]+cosy*ex[i]; + eys[i]=-cosy*en[i]-siny*ex[i]; + } + return 1; +} + + +/* phase windup model --------------------------------------------------------*/ +int model_phw(gtime_t time, int sat, const char *type, int opt, + const double *rs, const double *rr, double *phw) +{ + double exs[3],eys[3],ek[3],exr[3],eyr[3],eks[3],ekr[3],E[9]; + double dr[3],ds[3],drs[3],r[3],pos[3],cosp,ph; + int i; + + if (opt<=0) return 1; /* no phase windup */ + + /* satellite yaw attitude model */ + if (!sat_yaw(time,sat,type,opt,rs,exs,eys)) return 0; + + /* unit vector satellite to receiver */ + for (i=0;i<3;i++) r[i]=rr[i]-rs[i]; + if (!normv3(r,ek)) return 0; + + /* unit vectors of receiver antenna */ + ecef2pos(rr,pos); + xyz2enu(pos,E); + exr[0]= E[1]; exr[1]= E[4]; exr[2]= E[7]; /* x = north */ + eyr[0]=-E[0]; eyr[1]=-E[3]; eyr[2]=-E[6]; /* y = west */ + + /* phase windup effect */ + cross3(ek,eys,eks); + cross3(ek,eyr,ekr); + for (i=0;i<3;i++) + { + ds[i]=exs[i]-ek[i]*dot(ek,exs,3)-eks[i]; + dr[i]=exr[i]-ek[i]*dot(ek,exr,3)+ekr[i]; + } + cosp=dot(ds,dr,3)/norm(ds,3)/norm(dr,3); + if (cosp<-1.0) cosp=-1.0; + else if (cosp> 1.0) cosp= 1.0; + ph=acos(cosp)/2.0/PI; + cross3(ds,dr,drs); + if (dot(ek,drs,3)<0.0) ph=-ph; + + *phw=ph+floor(*phw-ph+0.5); /* in cycle */ + return 1; +} + + +/* measurement error variance ------------------------------------------------*/ +double varerr(int sat, int sys, double el, int freq, int type, + const prcopt_t *opt) +{ + double fact=1.0,sinel=sin(el); + + if (type==1) fact*=opt->eratio[freq==0?0:1]; + fact*=sys==SYS_GLO?EFACT_GLO:(sys==SYS_SBS?EFACT_SBS:EFACT_GPS); + + if (sys==SYS_GPS||sys==SYS_QZS) + { + if (freq==2) fact*=EFACT_GPS_L5; /* GPS/QZS L5 error factor */ + } + if (opt->ionoopt==IONOOPT_IFLC) fact*=3.0; + return std::pow(fact*opt->err[1], 2.0)+std::pow(fact*opt->err[2]/sinel, 2.0); +} + + +/* initialize state and covariance -------------------------------------------*/ +void initx(rtk_t *rtk, double xi, double var, int i) +{ + int j; + rtk->x[i]=xi; + for (j=0;jnx;j++) + { + rtk->P[i+j*rtk->nx]=rtk->P[j+i*rtk->nx]=i==j?var:0.0; + } +} + + +/* geometry-free phase measurement -------------------------------------------*/ +double gfmeas(const obsd_t *obs, const nav_t *nav) +{ + const double *lam=nav->lam[obs->sat-1]; + int i=(satsys(obs->sat,NULL)&(SYS_GAL|SYS_SBS))?2:1; + + if (lam[0]==0.0||lam[i]==0.0||obs->L[0]==0.0||obs->L[i]==0.0) return 0.0; + return lam[0]*obs->L[0]-lam[i]*obs->L[i]; +} + + +/* Melbourne-Wubbena linear combination --------------------------------------*/ +double mwmeas(const obsd_t *obs, const nav_t *nav) +{ + const double *lam=nav->lam[obs->sat-1]; + int i=(satsys(obs->sat,NULL)&(SYS_GAL|SYS_SBS))?2:1; + + if (lam[0]==0.0||lam[i]==0.0||obs->L[0]==0.0||obs->L[i]==0.0|| + obs->P[0]==0.0||obs->P[i]==0.0) return 0.0; + return lam[0]*lam[i]*(obs->L[0]-obs->L[i])/(lam[i]-lam[0])- + (lam[i]*obs->P[0]+lam[0]*obs->P[i])/(lam[i]+lam[0]); +} + + +/* antenna corrected measurements --------------------------------------------*/ +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) +{ + const double *lam=nav->lam[obs->sat-1]; + double C1,C2; + int i,sys; + + for (i=0;iL[i]==0.0||obs->P[i]==0.0) continue; + if (testsnr(0,0,azel[1],obs->SNR[i]*0.25,&opt->snrmask)) continue; + + /* antenna phase center and phase windup correction */ + L[i]=obs->L[i]*lam[i]-dants[i]-dantr[i]-phw*lam[i]; + P[i]=obs->P[i] -dants[i]-dantr[i]; + + /* P1-C1,P2-C2 dcb correction (C1->P1,C2->P2) */ + if (obs->code[i]==CODE_L1C) + { + P[i]+=nav->cbias[obs->sat-1][1]; + } + else if (obs->code[i]==CODE_L2C||obs->code[i]==CODE_L2X|| + obs->code[i]==CODE_L2L||obs->code[i]==CODE_L2S) + { + P[i]+=nav->cbias[obs->sat-1][2]; +#if 0 + L[i]-=0.25*lam[i]; /* 1/4 cycle-shift */ +#endif + } + } + /* iono-free LC */ + *Lc=*Pc=0.0; + sys=satsys(obs->sat,NULL); + i=(sys&(SYS_GAL|SYS_SBS))?2:1; /* L1/L2 or L1/L5 */ + if (lam[0]==0.0||lam[i]==0.0) return; + + C1= std::pow(lam[i], 2.0)/(std::pow(lam[i], 2.0)-std::pow(lam[0], 2.0)); + C2=-std::pow(lam[0], 2.0)/(std::pow(lam[i], 2.0)-std::pow(lam[0], 2.0)); + +#if 0 + /* P1-P2 dcb correction (P1->Pc,P2->Pc) */ + if (sys&(SYS_GPS|SYS_GLO|SYS_QZS)) + { + if (P[0]!=0.0) P[0]-=C2*nav->cbias[obs->sat-1][0]; + if (P[1]!=0.0) P[1]+=C1*nav->cbias[obs->sat-1][0]; + } +#endif + if (L[0]!=0.0&&L[i]!=0.0) *Lc=C1*L[0]+C2*L[i]; + if (P[0]!=0.0&&P[i]!=0.0) *Pc=C1*P[0]+C2*P[i]; +} + + +/* detect cycle slip by LLI --------------------------------------------------*/ +void detslp_ll(rtk_t *rtk, const obsd_t *obs, int n) +{ + int i,j; + + trace(3,"detslp_ll: n=%d\n",n); + + for (i=0;iopt.nf;j++) + { + if (obs[i].L[j]==0.0||!(obs[i].LLI[j]&3)) continue; + + trace(3,"detslp_ll: slip detected sat=%2d f=%d\n",obs[i].sat,j+1); + + rtk->ssat[obs[i].sat-1].slip[j]=1; + } +} + + +/* detect cycle slip by geometry free phase jump -----------------------------*/ +void detslp_gf(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) +{ + double g0,g1; + int i,j; + + trace(3,"detslp_gf: n=%d\n",n); + + for (i=0;issat[obs[i].sat-1].gf; + rtk->ssat[obs[i].sat-1].gf=g1; + + trace(4,"detslip_gf: sat=%2d gf0=%8.3f gf1=%8.3f\n",obs[i].sat,g0,g1); + + if (g0!=0.0&&fabs(g1-g0)>rtk->opt.thresslip) + { + trace(3,"detslip_gf: slip detected sat=%2d gf=%8.3f->%8.3f\n", + obs[i].sat,g0,g1); + + for (j=0;jopt.nf;j++) rtk->ssat[obs[i].sat-1].slip[j]|=1; + } + } +} + + +/* detect slip by Melbourne-Wubbena linear combination jump ------------------*/ +void detslp_mw(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) +{ + double w0,w1; + int i,j; + + trace(3,"detslp_mw: n=%d\n",n); + + for (i=0;issat[obs[i].sat-1].mw; + rtk->ssat[obs[i].sat-1].mw=w1; + + trace(4,"detslip_mw: sat=%2d mw0=%8.3f mw1=%8.3f\n",obs[i].sat,w0,w1); + + if (w0!=0.0&&fabs(w1-w0)>THRES_MW_JUMP) + { + trace(3,"detslip_mw: slip detected sat=%2d mw=%8.3f->%8.3f\n", + obs[i].sat,w0,w1); + + for (j=0;jopt.nf;j++) rtk->ssat[obs[i].sat-1].slip[j]|=1; + } + } +} + + +/* temporal update of position -----------------------------------------------*/ +void udpos_ppp(rtk_t *rtk) +{ + int i; + + trace(3,"udpos_ppp:\n"); + + /* fixed mode */ + if (rtk->opt.mode==PMODE_PPP_FIXED) + { + for (i=0;i<3;i++) initx(rtk,rtk->opt.ru[i],1E-8,i); + return; + } + /* initialize position for first epoch */ + if (norm(rtk->x,3)<=0.0) + { + for (i=0;i<3;i++) initx(rtk,rtk->sol.rr[i],VAR_POS_PPP,i); + } + /* static ppp mode */ + if (rtk->opt.mode==PMODE_PPP_STATIC) + { + for (i=0;i<3;i++) + { + rtk->P[i*(1+rtk->nx)]+=std::pow(rtk->opt.prn[5], 2.0)*fabs(rtk->tt); + } + return; + } + /* kinmatic mode without dynamics */ + for (i=0;i<3;i++) + { + initx(rtk,rtk->sol.rr[i],VAR_POS_PPP,i); + } +} + + +/* temporal update of clock --------------------------------------------------*/ +void udclk_ppp(rtk_t *rtk) +{ + double dtr; + int i; + + trace(3,"udclk_ppp:\n"); + + /* initialize every epoch for clock (white noise) */ + for (i=0;iopt.sateph==EPHOPT_PREC) + { + /* time of prec ephemeris is based gpst */ + /* negelect receiver inter-system bias */ + dtr=rtk->sol.dtr[0]; + } + else + { + dtr=i==0?rtk->sol.dtr[0]:rtk->sol.dtr[0]+rtk->sol.dtr[i]; + } + initx(rtk,SPEED_OF_LIGHT*dtr,VAR_CLK,IC_PPP(i,&rtk->opt)); + } +} + + +/* temporal update of tropospheric parameters --------------------------------*/ +void udtrop_ppp(rtk_t *rtk) +{ + double pos[3],azel[]={0.0,PI/2.0},ztd,var; + int i=IT_PPP(&rtk->opt),j; + + trace(3,"udtrop_ppp:\n"); + + if (rtk->x[i]==0.0) + { + ecef2pos(rtk->sol.rr,pos); + ztd=sbstropcorr(rtk->sol.time,pos,azel,&var); + initx(rtk,ztd,var,i); + + if (rtk->opt.tropopt>=TROPOPT_ESTG) + { + for (j=i+1;jP[i+i*rtk->nx]+=std::pow(rtk->opt.prn[2], 2.0)*fabs(rtk->tt); + + if (rtk->opt.tropopt>=TROPOPT_ESTG) + { + for (j=i+1;jP[j+j*rtk->nx]+=std::pow(rtk->opt.prn[2]*0.1, 2.0)*fabs(rtk->tt); + } + } + } +} + + +/* temporal update of ionospheric parameters ---------------------------------*/ +void udiono_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) +{ + const double *lam; + double ion,sinel,pos[3],*azel; + char *p; + int i,j,k,gap_resion=GAP_RESION; + + trace(3,"udiono_ppp:\n"); + + if ((p=strstr(rtk->opt.pppopt,"-GAP_RESION="))) + { + sscanf(p,"-GAP_RESION=%d",&gap_resion); + } + for (i=0;iopt); + if (rtk->x[j]!=0.0&&(int)rtk->ssat[i].outc[0]>gap_resion) + { + rtk->x[j]=0.0; + } + } + for (i=0;iopt); + if (rtk->x[j]==0.0) + { + k=satsys(obs[i].sat,NULL)==SYS_GAL?2:1; + lam=nav->lam[obs[i].sat-1]; + if (obs[i].P[0]==0.0||obs[i].P[k]==0.0||lam[0]==0.0||lam[k]==0.0) + { + continue; + } + ion=(obs[i].P[0]-obs[i].P[k])/(1.0-std::pow(lam[k]/lam[0], 2.0)); + ecef2pos(rtk->sol.rr,pos); + azel=rtk->ssat[obs[i].sat-1].azel; + ion/=ionmapf(pos,azel); + initx(rtk,ion,VAR_IONO,j); + } + else + { + sinel=sin(MAX_PPP(rtk->ssat[obs[i].sat-1].azel[1],5.0*D2R)); + rtk->P[j+j*rtk->nx]+=std::pow(rtk->opt.prn[1]/sinel, 2.0)*fabs(rtk->tt); + } + } +} + + +/* temporal update of L5-receiver-dcb parameters -----------------------------*/ +void uddcb_ppp(rtk_t *rtk) +{ + int i=ID_PPP(&rtk->opt); + + trace(3,"uddcb_ppp:\n"); + + if (rtk->x[i]==0.0) + { + initx(rtk,1E-6,VAR_DCB,i); + } +} + + +/* temporal update of phase biases -------------------------------------------*/ +void udbias_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) +{ + const double *lam; + double L[NFREQ],P[NFREQ],Lc,Pc,bias[MAXOBS],offset=0.0,pos[3]={0}; + double ion,dantr[NFREQ]={0},dants[NFREQ]={0}; + int i,j,k,l,f,sat,slip[MAXOBS]={0},clk_jump=0; + + trace(3,"udbias : n=%d\n",n); + + /* handle day-boundary clock jump */ + if (rtk->opt.posopt[5]) + { + clk_jump=ROUND_PPP(time2gpst(obs[0].time,NULL)*10)%864000==0; + } + for (i=0;iopt.nf;j++) + { + rtk->ssat[i].slip[j]=0; + } + /* detect cycle slip by LLI */ + detslp_ll(rtk,obs,n); + + /* detect cycle slip by geometry-free phase jump */ + detslp_gf(rtk,obs,n,nav); + + /* detect slip by Melbourne-Wubbena linear combination jump */ + detslp_mw(rtk,obs,n,nav); + + ecef2pos(rtk->sol.rr,pos); + + for (f=0;fopt);f++) + { + + /* reset phase-bias if expire obs outage counter */ + for (i=0;issat[i].outc[f]>(unsigned int)rtk->opt.maxout|| + rtk->opt.modear==ARMODE_INST||clk_jump) + { + initx(rtk,0.0,0.0,IB_PPP(i+1,f,&rtk->opt)); + } + } + for (i=k=0;iopt); + corr_meas(obs+i,nav,rtk->ssat[sat-1].azel,&rtk->opt,dantr,dants, + 0.0,L,P,&Lc,&Pc); + + bias[i]=0.0; + + if (rtk->opt.ionoopt==IONOOPT_IFLC) + { + bias[i]=Lc-Pc; + slip[i]=rtk->ssat[sat-1].slip[0]||rtk->ssat[sat-1].slip[1]; + } + else if (L[f]!=0.0&&P[f]!=0.0) + { + slip[i]=rtk->ssat[sat-1].slip[f]; + l=satsys(sat,NULL)==SYS_GAL?2:1; + lam=nav->lam[sat-1]; + if (obs[i].P[0]==0.0||obs[i].P[l]==0.0|| + lam[0]==0.0||lam[l]==0.0||lam[f]==0.0) continue; + ion=(obs[i].P[0]-obs[i].P[l])/(1.0-std::pow(lam[l]/lam[0], 2.0)); + bias[i]=L[f]-P[f]+2.0*ion*std::pow(lam[f]/lam[0], 2.0); + } + if (rtk->x[j]==0.0||slip[i]||bias[i]==0.0) continue; + + offset+=bias[i]-rtk->x[j]; + k++; + } + /* correct phase-code jump to ensure phase-code coherency */ + if (k>=2&&fabs(offset/k)>0.0005*SPEED_OF_LIGHT) + { + for (i=0;iopt); + if (rtk->x[j]!=0.0) rtk->x[j]+=offset/k; + } + trace(2,"phase-code jump corrected: %s n=%2d dt=%12.9fs\n", + time_str(rtk->sol.time,0),k,offset/k/SPEED_OF_LIGHT); + } + for (i=0;iopt); + + rtk->P[j+j*rtk->nx]+=std::pow(rtk->opt.prn[0], 2.0)*fabs(rtk->tt); + + if (bias[i]==0.0||(rtk->x[j]!=0.0&&!slip[i])) continue; + + /* reinitialize phase-bias if detecting cycle slip */ + initx(rtk,bias[i],VAR_BIAS,IB_PPP(sat,f,&rtk->opt)); + + /* reset fix flags */ + for (k=0;kambc[sat-1].flags[k]=0; + + trace(5,"udbias_ppp: sat=%2d bias=%.3f\n",sat,bias[i]); + } + } +} + + +/* temporal update of states --------------------------------------------------*/ +void udstate_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) +{ + trace(3,"udstate_ppp: n=%d\n",n); + + /* temporal update of position */ + udpos_ppp(rtk); + + /* temporal update of clock */ + udclk_ppp(rtk); + + /* temporal update of tropospheric parameters */ + if (rtk->opt.tropopt==TROPOPT_EST||rtk->opt.tropopt==TROPOPT_ESTG) + { + udtrop_ppp(rtk); + } + /* temporal update of ionospheric parameters */ + if (rtk->opt.ionoopt==IONOOPT_EST) + { + udiono_ppp(rtk,obs,n,nav); + } + /* temporal update of L5-receiver-dcb parameters */ + if (rtk->opt.nf>=3) + { + uddcb_ppp(rtk); + } + /* temporal update of phase-bias */ + udbias_ppp(rtk,obs,n,nav); +} + + +/* satellite antenna phase center variation ----------------------------------*/ +void satantpcv(const double *rs, const double *rr, const pcv_t *pcv, + double *dant) +{ + double ru[3],rz[3],eu[3],ez[3],nadir,cosa; + int i; + + for (i=0;i<3;i++) + { + ru[i]=rr[i]-rs[i]; + rz[i]=-rs[i]; + } + if (!normv3(ru,eu)||!normv3(rz,ez)) return; + + cosa=dot(eu,ez,3); + cosa=cosa<-1.0?-1.0:(cosa>1.0?1.0:cosa); + nadir=acos(cosa); + + antmodel_s(pcv,nadir,dant); +} + + +/* precise tropospheric model ------------------------------------------------*/ +double trop_model_prec(gtime_t time, const double *pos, + const double *azel, const double *x, double *dtdx, + double *var) +{ + const double zazel[]={0.0,PI/2.0}; + double zhd,m_h,m_w,cotz,grad_n,grad_e; + + /* zenith hydrostatic delay */ + zhd=tropmodel(time,pos,zazel,0.0); + + /* mapping function */ + m_h=tropmapf(time,pos,azel,&m_w); + + if (azel[1]>0.0) + { + /* m_w=m_0+m_0*cot(el)*(Gn*cos(az)+Ge*sin(az)): ref [6] */ + cotz=1.0/tan(azel[1]); + grad_n=m_w*cotz*cos(azel[0]); + grad_e=m_w*cotz*sin(azel[0]); + m_w+=grad_n*x[1]+grad_e*x[2]; + dtdx[1]=grad_n*(x[0]-zhd); + dtdx[2]=grad_e*(x[0]-zhd); + } + dtdx[0]=m_w; + *var=std::pow(0.01, 2.0); + return m_h*zhd+m_w*(x[0]-zhd); +} + + +/* tropospheric model ---------------------------------------------------------*/ +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) +{ + double trp[3]={0},std[3]; + + if (opt->tropopt==TROPOPT_SAAS) + { + *dtrp=tropmodel(time,pos,azel,REL_HUMI); + *var=std::pow(ERR_SAAS, 2.0); + return 1; + } + if (opt->tropopt==TROPOPT_SBAS) + { + *dtrp=sbstropcorr(time,pos,azel,var); + return 1; + } + if (opt->tropopt==TROPOPT_EST||opt->tropopt==TROPOPT_ESTG) + { + matcpy(trp,x+IT_PPP(opt),opt->tropopt==TROPOPT_EST?1:3,1); + *dtrp=trop_model_prec(time,pos,azel,trp,dtdx,var); + return 1; + } + if (opt->tropopt==TROPOPT_ZTD) + { + if (pppcorr_trop(&nav->pppcorr,time,pos,trp,std)) + { + *dtrp=trop_model_prec(time,pos,azel,trp,dtdx,var); + *var=std::pow(dtdx[0]*std[0], 2.0); + return 1; + } + } + return 0; +} + + +/* ionospheric model ---------------------------------------------------------*/ +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) +{ + static double iono_p[MAXSAT]={0},std_p[MAXSAT]={0}; + static gtime_t time_p; + + if (opt->ionoopt==IONOOPT_SBAS) + { + return sbsioncorr(time,nav,pos,azel,dion,var); + } + if (opt->ionoopt==IONOOPT_TEC) + { + return iontec(time,nav,pos,azel,1,dion,var); + } + if (opt->ionoopt==IONOOPT_BRDC) + { + *dion=ionmodel(time,nav->ion_gps,pos,azel); + *var=std::pow(*dion*ERR_BRDCI, 2.0); + return 1; + } + if (opt->ionoopt==IONOOPT_EST) + { + *dion=x[II_PPP(sat,opt)]; + *var=0.0; + return 1; + } + if (opt->ionoopt==IONOOPT_IFLC) + { + *dion=*var=0.0; + return 1; + } + if (opt->ionoopt==IONOOPT_STEC) + { + if (timediff(time,time_p)!=0.0&& + !pppcorr_stec(&nav->pppcorr,time,pos,iono_p,std_p)) return 0; + if (iono_p[sat-1]==0.0||std_p[sat-1]>0.1) return 0; + time_p=time; + *dion=iono_p[sat-1]; + *var=std::pow(std_p[sat-1], 2.0); + return 1; + } + return 0; +} + + +/* constraint to local correction --------------------------------------------*/ +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) +{ + gtime_t time=obs[0].time; + double trop[3],std_trop[3],iono[MAXSAT],std_iono[MAXSAT]; + int i,j,k,sat,nv=0; + + /* constraint to external troposphere correction */ + if ((rtk->opt.tropopt==TROPOPT_EST||rtk->opt.tropopt==TROPOPT_ESTG)&& + pppcorr_trop(&nav->pppcorr,time,pos,trop,std_trop)) + { + + for (i=0;i<(rtk->opt.tropopt==TROPOPT_EST?1:3);i++) + { + if (std_trop[i]==0.0) continue; + j=IT_PPP(&rtk->opt)+i; + v[nv]=trop[i]-x[j]; + for (k=0;knx;k++) H[k+nv*rtk->nx]=k==j?1.0:0.0; + var[nv++]=std::pow(std_trop[i], 2.0); + } + } + /* constraint to external ionosphere correction */ + if (rtk->opt.ionoopt==IONOOPT_EST&& + pppcorr_stec(&nav->pppcorr,time,pos,iono,std_iono)) + { + + for (i=0;i0.5) continue; + j=II_PPP(sat,&rtk->opt); + v[nv]=iono[sat-1]-x[j]; + for (k=0;knx;k++) H[k+nv*rtk->nx]=k==j?1.0:0.0; + var[nv++]=std::pow(std_iono[sat-1], 2.0); + } + } + return nv; +} + + +/* phase and code residuals --------------------------------------------------*/ +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) +{ + const double *lam; + prcopt_t *opt=&rtk->opt; + double y,r,cdtr,bias,C,rr[3],pos[3],e[3],dtdx[3],L[NFREQ],P[NFREQ],Lc,Pc; + double var[MAXOBS*2],dtrp=0.0,dion=0.0,vart=0.0,vari=0.0,dcb; + double dantr[NFREQ]={0},dants[NFREQ]={0}; + double ve[MAXOBS*2*NFREQ]={0},vmax=0; + char str[32]; + int ne=0,obsi[MAXOBS*2*NFREQ]={0},frqi[MAXOBS*2*NFREQ],maxobs,maxfrq,rej; + int i,j,k,sat,sys,nv=0,nx=rtk->nx,stat=1; + + time2str(obs[0].time,str,2); + + for (i=0;inf;j++) rtk->ssat[i].vsat[j]=0; + + for (i=0;i<3;i++) rr[i]=x[i]+dr[i]; + ecef2pos(rr,pos); + + for (i=0;ilam[sat-1]; + if (lam[j/2]==0.0||lam[0]==0.0) continue; + + if ((r=geodist(rs+i*6,rr,e))<=0.0|| + satazel(pos,e,azel+i*2)elmin) + { + exc[i]=1; + continue; + } + if (!(sys=satsys(sat,NULL))||!rtk->ssat[sat-1].vs|| + satexclude(obs[i].sat,svh[i],opt)||exc[i]) + { + exc[i]=1; + continue; + } + /* tropospheric and ionospheric model */ + if (!model_trop(obs[i].time,pos,azel+i*2,opt,x,dtdx,nav,&dtrp,&vart)|| + !model_iono(obs[i].time,pos,azel+i*2,opt,sat,x,nav,&dion,&vari)) + { + continue; + } + /* satellite and receiver antenna model */ + if (opt->posopt[0]) satantpcv(rs+i*6,rr,nav->pcvs+sat-1,dants); + antmodel(opt->pcvr,opt->antdel[0],azel+i*2,opt->posopt[1],dantr); + + /* phase windup model */ + if (!model_phw(rtk->sol.time,sat,nav->pcvs[sat-1].type, + opt->posopt[2]?2:0,rs+i*6,rr,&rtk->ssat[sat-1].phw)) + { + continue; + } + /* corrected phase and code measurements */ + corr_meas(obs+i,nav,azel+i*2,&rtk->opt,dantr,dants, + rtk->ssat[sat-1].phw,L,P,&Lc,&Pc); + + /* stack phase and code residuals {L1,P1,L2,P2,...} */ + for (j=0;j<2*NF_PPP(opt);j++) { + + dcb=bias=0.0; + + if (opt->ionoopt==IONOOPT_IFLC) + { + if ((y=j%2==0?Lc:Pc)==0.0) continue; + } + else + { + if ((y=j%2==0?L[j/2]:P[j/2])==0.0) continue; + + /* receiver DCB correction for P2 */ + if (j/2==1) dcb=-nav->rbias[0][sys==SYS_GLO?1:0][0]; + } + C=std::pow(lam[j/2]/lam[0], 2.0)*ionmapf(pos,azel+i*2)*(j%2==0?-1.0:1.0); + + for (k=0;ktropopt==TROPOPT_EST||opt->tropopt==TROPOPT_ESTG) + { + for (k=0;k<(opt->tropopt>=TROPOPT_ESTG?3:1);k++) + { + H[IT_PPP(opt)+k+nx*nv]=dtdx[k]; + } + } + if (opt->ionoopt==IONOOPT_EST) + { + if (rtk->x[II_PPP(sat,opt)]==0.0) continue; + H[II_PPP(sat,opt)+nx*nv]=C; + } + if (j/2==2&&j%2==1) + { /* L5-receiver-dcb */ + dcb+=rtk->x[ID_PPP(opt)]; + H[ID_PPP(opt)+nx*nv]=1.0; + } + if (j%2==0) + { /* phase bias */ + if ((bias=x[IB_PPP(sat,j/2,opt)])==0.0) continue; + H[IB_PPP(sat,j/2,opt)+nx*nv]=1.0; + } + /* residual */ + v[nv]=y-(r+cdtr-SPEED_OF_LIGHT*dts[i*2]+dtrp+C*dion+dcb+bias); + + if (j%2==0) rtk->ssat[sat-1].resc[j/2]=v[nv]; + else rtk->ssat[sat-1].resp[j/2]=v[nv]; + + /* variance */ + var[nv]=varerr(obs[i].sat,sys,azel[1+i*2],j/2,j%2,opt)+ + vart+std::pow(C, 2.0)*vari+var_rs[i]; + if (sys==SYS_GLO&&j%2==1) var[nv]+=VAR_GLO_IFB; + + trace(4,"%s sat=%2d %s%d res=%9.4f sig=%9.4f el=%4.1f\n",str,sat, + j%2?"P":"L",j/2+1,v[nv],sqrt(var[nv]),azel[1+i*2]*R2D); + + /* reject satellite by pre-fit residuals */ + if (!post&&opt->maxinno>0.0&&fabs(v[nv])>opt->maxinno) + { + trace(2,"outlier (%d) rejected %s sat=%2d %s%d res=%9.4f el=%4.1f\n", + post,str,sat,j%2?"P":"L",j/2+1,v[nv],azel[1+i*2]*R2D); + exc[i]=1; rtk->ssat[sat-1].rejc[j%2]++; + continue; + } + /* record large post-fit residuals */ + if (post&&fabs(v[nv])>sqrt(var[nv])*THRES_REJECT) + { + obsi[ne]=i; frqi[ne]=j; ve[ne]=v[nv]; ne++; + } + if (j%2==0) rtk->ssat[sat-1].vsat[j/2]=1; + nv++; + } + } + /* reject satellite with large and max post-fit residual */ + if (post&&ne>0) + { + vmax=ve[0]; maxobs=obsi[0]; maxfrq=frqi[0]; rej=0; + for (j=1;j=fabs(ve[j])) continue; + vmax=ve[j]; maxobs=obsi[j]; maxfrq=frqi[j]; rej=j; + } + sat=obs[maxobs].sat; + trace(2,"outlier (%d) rejected %s sat=%2d %s%d res=%9.4f el=%4.1f\n", + post,str,sat,maxfrq%2?"P":"L",maxfrq/2+1,vmax,azel[1+maxobs*2]*R2D); + exc[maxobs]=1; rtk->ssat[sat-1].rejc[maxfrq%2]++; stat=0; + ve[rej]=0; + } + /* constraint to local correction */ + nv+=const_corr(obs,n,exc,nav,x,pos,azel,rtk,v+nv,H+nv*rtk->nx,var+nv); + + for (i=0;iopt; + int i,j; + + /* test # of valid satellites */ + rtk->sol.ns=0; + for (i=0;inf;j++) + { + if (!rtk->ssat[obs[i].sat-1].vsat[j]) continue; + rtk->ssat[obs[i].sat-1].lock[j]++; + rtk->ssat[obs[i].sat-1].outc[j]=0; + if (j==0) rtk->sol.ns++; + } + } + rtk->sol.stat=rtk->sol.nssol.stat==SOLQ_FIX) + { + for (i=0;i<3;i++) + { + rtk->sol.rr[i]=rtk->xa[i]; + rtk->sol.qr[i]=(float)rtk->Pa[i+i*rtk->na]; + } + rtk->sol.qr[3]=(float)rtk->Pa[1]; + rtk->sol.qr[4]=(float)rtk->Pa[1+2*rtk->na]; + rtk->sol.qr[5]=(float)rtk->Pa[2]; + } + else + { + for (i=0;i<3;i++) + { + rtk->sol.rr[i]=rtk->x[i]; + rtk->sol.qr[i]=(float)rtk->P[i+i*rtk->nx]; + } + rtk->sol.qr[3]=(float)rtk->P[1]; + rtk->sol.qr[4]=(float)rtk->P[2+rtk->nx]; + rtk->sol.qr[5]=(float)rtk->P[2]; + } + rtk->sol.dtr[0]=rtk->x[IC_PPP(0,opt)]; + rtk->sol.dtr[1]=rtk->x[IC_PPP(1,opt)]-rtk->x[IC_PPP(0,opt)]; + + for (i=0;inf;j++) + { + rtk->ssat[obs[i].sat-1].snr[j]=obs[i].SNR[j]; + } + for (i=0;inf;j++) + { + if (rtk->ssat[i].slip[j]&3) rtk->ssat[i].slipc[j]++; + if (rtk->ssat[i].fix[j]==2&&stat!=SOLQ_FIX) rtk->ssat[i].fix[j]=1; + } +} + + +/* test hold ambiguity -------------------------------------------------------*/ +int test_hold_amb(rtk_t *rtk) +{ + int i,j,stat=0; + + /* no fix-and-hold mode */ + if (rtk->opt.modear!=ARMODE_FIXHOLD) return 0; + + /* reset # of continuous fixed if new ambiguity introduced */ + for (i=0;issat[i].fix[0]!=2&&rtk->ssat[i].fix[1]!=2) continue; + for (j=0;jssat[j].fix[0]!=2&&rtk->ssat[j].fix[1]!=2) continue; + if (!rtk->ambc[j].flags[i]||!rtk->ambc[i].flags[j]) stat=1; + rtk->ambc[j].flags[i]=rtk->ambc[i].flags[j]=1; + } + } + if (stat) + { + rtk->nfix=0; + return 0; + } + /* test # of continuous fixed */ + return ++rtk->nfix>=rtk->opt.minfix; +} + + +/* precise point positioning -------------------------------------------------*/ +void pppos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) +{ + const prcopt_t *opt=&rtk->opt; + double *rs,*dts,*var,*v,*H,*R,*azel,*xp,*Pp,dr[3]={0},std[3]; + char str[32]; + int i,j,nv,info,svh[MAXOBS],exc[MAXOBS]={0},stat=SOLQ_SINGLE; + + time2str(obs[0].time,str,2); + trace(3,"pppos : time=%s nx=%d n=%d\n",str,rtk->nx,n); + + rs=mat(6,n); dts=mat(2,n); var=mat(1,n); azel=zeros(2,n); + + for (i=0;inf;j++) rtk->ssat[i].fix[j]=0; + + /* temporal update of ekf states */ + udstate_ppp(rtk,obs,n,nav); + + /* satellite positions and clocks */ + satposs(obs[0].time,obs,n,nav,rtk->opt.sateph,rs,dts,var,svh); + + /* exclude measurements of eclipsing satellite (block IIA) */ + if (rtk->opt.posopt[3]) + { + testeclipse(obs,n,nav,rs); + } + /* earth tides correction */ + if (opt->tidecorr) + { + tidedisp(gpst2utc(obs[0].time),rtk->x,opt->tidecorr==1?1:7,&nav->erp, + opt->odisp[0],dr); + } + nv=n*rtk->opt.nf*2+MAXSAT+3; + xp=mat(rtk->nx,1); Pp=zeros(rtk->nx,rtk->nx); + v=mat(nv,1); H=mat(rtk->nx,nv); R=mat(nv,nv); + + for (i=0;ix,rtk->nx,1); + matcpy(Pp,rtk->P,rtk->nx,rtk->nx); + + /* prefit residuals */ + if (!(nv=ppp_res(0,obs,n,rs,dts,var,svh,dr,exc,nav,xp,rtk,v,H,R,azel))) + { + trace(2,"%s ppp (%d) no valid obs data\n",str,i+1); + break; + } + /* measurement update of ekf states */ + if ((info=filter(xp,Pp,H,v,R,rtk->nx,nv))) + { + trace(2,"%s ppp (%d) filter error info=%d\n",str,i+1,info); + break; + } + /* postfit residuals */ + if (ppp_res(i+1,obs,n,rs,dts,var,svh,dr,exc,nav,xp,rtk,v,H,R,azel)) + { + matcpy(rtk->x,xp,rtk->nx,1); + matcpy(rtk->P,Pp,rtk->nx,rtk->nx); + stat=SOLQ_PPP; + break; + } + } + if (i>=MAX_ITER) + { + trace(2,"%s ppp (%d) iteration overflows\n",str,i); + } + if (stat==SOLQ_PPP) + { + + /* ambiguity resolution in ppp */ + if (ppp_ar(rtk,obs,n,exc,nav,azel,xp,Pp)&& + ppp_res(9,obs,n,rs,dts,var,svh,dr,exc,nav,xp,rtk,v,H,R,azel)) + { + matcpy(rtk->xa,xp,rtk->nx,1); + matcpy(rtk->Pa,Pp,rtk->nx,rtk->nx); + + for (i=0;i<3;i++) std[i]=sqrt(Pp[i+i*rtk->nx]); + if (norm(std,3)nfix=0; + } + /* update solution status */ + update_stat(rtk,obs,n,stat); + + /* hold fixed ambiguities */ + if (stat==SOLQ_FIX&&test_hold_amb(rtk)) + { + matcpy(rtk->x,xp,rtk->nx,1); + matcpy(rtk->P,Pp,rtk->nx,rtk->nx); + trace(2,"%s hold ambiguity\n",str); + rtk->nfix=0; + } + } + free(rs); free(dts); free(var); free(azel); + free(xp); free(Pp); free(v); free(H); free(R); +} diff --git a/src/algorithms/libs/rtklib/rtklib_ppp.h b/src/algorithms/libs/rtklib/rtklib_ppp.h new file mode 100644 index 000000000..45b2f99c7 --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_ppp.h @@ -0,0 +1,199 @@ +/*! + * \file rtklib_ppp.h + * \brief Precise Point Positioning + * \authors
      + *
    • 2007-2008, T. Takasu + *
    • 2017, Javier Arribas + *
    • 2017, Carles Fernandez + *
    + * + * 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)->tropopttropopt==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 diff --git a/src/algorithms/libs/rtklib/rtklib_preceph.cc b/src/algorithms/libs/rtklib/rtklib_preceph.cc index 9fea57f2c..6973a513b 100644 --- a/src/algorithms/libs/rtklib/rtklib_preceph.cc +++ b/src/algorithms/libs/rtklib/rtklib_preceph.cc @@ -61,7 +61,7 @@ *----------------------------------------------------------------------------*/ #include "rtklib_preceph.h" - +#include "rtklib_rtkcmn.h" /* satellite code to satellite system ----------------------------------------*/ int code2sys(char code) diff --git a/src/algorithms/libs/rtklib/rtklib_preceph.h b/src/algorithms/libs/rtklib/rtklib_preceph.h index 673a96bf8..056963916 100644 --- a/src/algorithms/libs/rtklib/rtklib_preceph.h +++ b/src/algorithms/libs/rtklib/rtklib_preceph.h @@ -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 */ diff --git a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc index bf3d1599e..b843e2c45 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc @@ -50,14 +50,15 @@ * *----------------------------------------------------------------------------*/ -#include -#include +#include "rtklib_rtkcmn.h" +//#include +//#include #include -#include +//#include #include #include #include -#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 */ } } diff --git a/src/algorithms/libs/rtklib/rtklib_rtkcmn.h b/src/algorithms/libs/rtklib/rtklib_rtkcmn.h index 6a8383fe1..c3c8d64bc 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkcmn.h +++ b/src/algorithms/libs/rtklib/rtklib_rtkcmn.h @@ -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); diff --git a/src/algorithms/libs/rtklib/rtklib_rtkpos.cc b/src/algorithms/libs/rtklib/rtklib_rtkpos.cc index f2b77d949..9743df1a5 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkpos.cc @@ -52,708 +52,2084 @@ *----------------------------------------------------------------------------*/ #include "rtklib_rtkpos.h" -#include "rtklib_rtkcmn.h" -#include "rtklib_sbas.h" -#include "rtklib_ionex.h" -#include "rtklib_ephemeris.h" #include "rtklib_pntpos.h" +#include "rtklib_ephemeris.h" +#include "rtklib_ppp.h" +#include "rtklib_tides.h" +#include "rtklib_lambda.h" -/* pseudorange measurement error variance ------------------------------------*/ - double varerr(const prcopt_t *opt, double el, int sys) +/* open solution status file --------------------------------------------------- + * open solution status file and set output level + * args : char *file I rtk status file + * int level I rtk status level (0: off) + * return : status (1:ok,0:error) + * notes : file can constain time keywords (%Y,%y,%m...) defined in reppath(). + * The time to replace keywords is based on UTC of CPU time. + * output : solution status file record format + * + * $POS,week,tow,stat,posx,posy,posz,posxf,posyf,poszf + * week/tow : gps week no/time of week (s) + * stat : solution status + * posx/posy/posz : position x/y/z ecef (m) float + * posxf/posyf/poszf : position x/y/z ecef (m) fixed + * + * $VELACC,week,tow,stat,vele,veln,velu,acce,accn,accu,velef,velnf,veluf,accef,accnf,accuf + * week/tow : gps week no/time of week (s) + * stat : solution status + * vele/veln/velu : velocity e/n/u (m/s) float + * acce/accn/accu : acceleration e/n/u (m/s^2) float + * velef/velnf/veluf : velocity e/n/u (m/s) fixed + * accef/accnf/accuf : acceleration e/n/u (m/s^2) fixed + * + * $CLK,week,tow,stat,clk1,clk2,clk3,clk4 + * week/tow : gps week no/time of week (s) + * stat : solution status + * clk1 : receiver clock bias GPS (ns) + * clk2 : receiver clock bias GLO-GPS (ns) + * clk3 : receiver clock bias GAL-GPS (ns) + * clk4 : receiver clock bias BDS-GPS (ns) + * + * $ION,week,tow,stat,sat,az,el,ion,ion-fixed + * week/tow : gps week no/time of week (s) + * stat : solution status + * sat : satellite id + * az/el : azimuth/elevation angle(deg) + * ion : vertical ionospheric delay L1 (m) float + * ion-fixed: vertical ionospheric delay L1 (m) fixed + * + * $TROP,week,tow,stat,rcv,ztd,ztdf + * week/tow : gps week no/time of week (s) + * stat : solution status + * rcv : receiver (1:rover,2:base station) + * ztd : zenith total delay (m) float + * ztdf : zenith total delay (m) fixed + * + * $HWBIAS,week,tow,stat,frq,bias,biasf + * week/tow : gps week no/time of week (s) + * stat : solution status + * frq : frequency (1:L1,2:L2,...) + * bias : h/w bias coefficient (m/MHz) float + * biasf : h/w bias coefficient (m/MHz) fixed + * + * $SAT,week,tow,sat,frq,az,el,resp,resc,vsat,snr,fix,slip,lock,outc,slipc,rejc + * week/tow : gps week no/time of week (s) + * sat/frq : satellite id/frequency (1:L1,2:L2,...) + * az/el : azimuth/elevation angle (deg) + * resp : pseudorange residual (m) + * resc : carrier-phase residual (m) + * vsat : valid data flag (0:invalid,1:valid) + * snr : signal strength (dbHz) + * fix : ambiguity flag (0:no data,1:float,2:fixed,3:hold,4:ppp) + * slip : cycle-slip flag (bit1:slip,bit2:parity unknown) + * lock : carrier-lock count + * outc : data outage count + * slipc : cycle-slip count + * rejc : data reject (outlier) count + * + *-----------------------------------------------------------------------------*/ +int rtkopenstat(const char *file, int level) { - double fact, varr; - fact = sys == SYS_GLO ? EFACT_GLO : (sys == SYS_SBS ? EFACT_SBS : EFACT_GPS); - varr = std::pow(opt->err[0], 2.0)*(std::pow(opt->err[1], 2.0)+std::pow(opt->err[2], 2.0) / sin(el)); - if (opt->ionoopt == IONOOPT_IFLC) varr *= std::pow(3.0, 2.0); /* iono-free */ - return std::pow(fact, 2.0)*varr; + gtime_t time=utc2gpst(timeget()); + char path[1024]; + + trace(3,"rtkopenstat: file=%s level=%d\n",file,level); + + if (level<=0) return 0; + + reppath(file,path,time,"",""); + + if (!(fp_stat=fopen(path,"w"))) + { + trace(1,"rtkopenstat: file open error path=%s\n",path); + return 0; + } + strcpy(file_stat,file); + time_stat=time; + statlevel=level; + return 1; } -/* get tgd parameter (m) -----------------------------------------------------*/ - double gettgd(int sat, const nav_t *nav) +/* close solution status file -------------------------------------------------- + * close solution status file + * args : none + * return : none + *-----------------------------------------------------------------------------*/ +void rtkclosestat(void) +{ + trace(3,"rtkclosestat:\n"); + + if (fp_stat) fclose(fp_stat); + fp_stat=NULL; + file_stat[0]='\0'; + statlevel=0; +} + + +/* write solution status to buffer -------------------------------------------*/ +int rtkoutstat(rtk_t *rtk, char *buff) +{ + ssat_t *ssat; + double tow,pos[3],vel[3],acc[3],vela[3]={0},acca[3]={0},xa[3]; + int i,j,week,est,nfreq,nf=NF_RTK(&rtk->opt); + char id[32],*p=buff; + + if (rtk->sol.stat<=SOLQ_NONE) + { + return 0; + } + /* write ppp solution status to buffer */ + if (rtk->opt.mode>=PMODE_PPP_KINEMA) + { + return pppoutstat(rtk,buff); + } + est=rtk->opt.mode>=PMODE_DGPS; + nfreq=est?nf:1; + tow=time2gpst(rtk->sol.time,&week); + + /* receiver position */ + if (est) + { + for (i=0;i<3;i++) xa[i]=ina?rtk->xa[i]:0.0; + p+=sprintf(p,"$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n",week,tow, + rtk->sol.stat,rtk->x[0],rtk->x[1],rtk->x[2],xa[0],xa[1], + xa[2]); + } + else + { + p+=sprintf(p,"$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n",week,tow, + rtk->sol.stat,rtk->sol.rr[0],rtk->sol.rr[1],rtk->sol.rr[2], + 0.0,0.0,0.0); + } + /* receiver velocity and acceleration */ + if (est&&rtk->opt.dynamics) + { + ecef2pos(rtk->sol.rr,pos); + ecef2enu(pos,rtk->x+3,vel); + ecef2enu(pos,rtk->x+6,acc); + if (rtk->na>=6) ecef2enu(pos,rtk->xa+3,vela); + if (rtk->na>=9) ecef2enu(pos,rtk->xa+6,acca); + p+=sprintf(p,"$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n", + week,tow,rtk->sol.stat,vel[0],vel[1],vel[2],acc[0],acc[1], + acc[2],vela[0],vela[1],vela[2],acca[0],acca[1],acca[2]); + } + else + { + ecef2pos(rtk->sol.rr,pos); + ecef2enu(pos,rtk->sol.rr+3,vel); + p+=sprintf(p,"$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n", + week,tow,rtk->sol.stat,vel[0],vel[1],vel[2], + 0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0); + } + /* receiver clocks */ + p+=sprintf(p,"$CLK,%d,%.3f,%d,%d,%.3f,%.3f,%.3f,%.3f\n", + week,tow,rtk->sol.stat,1,rtk->sol.dtr[0]*1E9,rtk->sol.dtr[1]*1E9, + rtk->sol.dtr[2]*1E9,rtk->sol.dtr[3]*1E9); + + /* ionospheric parameters */ + if (est&&rtk->opt.ionoopt==IONOOPT_EST) + { + for (i=0;issat+i; + if (!ssat->vs) continue; + satno2id(i+1,id); + j=II_RTK(i+1,&rtk->opt); + xa[0]=jna?rtk->xa[j]:0.0; + p+=sprintf(p,"$ION,%d,%.3f,%d,%s,%.1f,%.1f,%.4f,%.4f\n",week,tow, + rtk->sol.stat,id,ssat->azel[0]*R2D,ssat->azel[1]*R2D, + rtk->x[j],xa[0]); + } + } + /* tropospheric parameters */ + if (est&&(rtk->opt.tropopt==TROPOPT_EST||rtk->opt.tropopt==TROPOPT_ESTG)) + { + for (i=0;i<2;i++) { + j=IT_RTK(i,&rtk->opt); + xa[0]=jna?rtk->xa[j]:0.0; + p+=sprintf(p,"$TROP,%d,%.3f,%d,%d,%.4f,%.4f\n",week,tow, + rtk->sol.stat,i+1,rtk->x[j],xa[0]); + } + } + /* receiver h/w bias */ + if (est&&rtk->opt.glomodear==2) + { + for (i=0;iopt); + xa[0]=jna?rtk->xa[j]:0.0; + p+=sprintf(p,"$HWBIAS,%d,%.3f,%d,%d,%.4f,%.4f\n",week,tow, + rtk->sol.stat,i+1,rtk->x[j],xa[0]); + } + } + return (int)(p-buff); +} + + +/* swap solution status file -------------------------------------------------*/ +void swapsolstat(void) +{ + gtime_t time=utc2gpst(timeget()); + char path[1024]; + + if ((int)(time2gpst(time ,NULL)/INT_SWAP_STAT)== + (int)(time2gpst(time_stat,NULL)/INT_SWAP_STAT)) + { + return; + } + time_stat=time; + + if (!reppath(file_stat,path,time,"","")) + { + return; + } + if (fp_stat) fclose(fp_stat); + + if (!(fp_stat=fopen(path,"w"))) + { + trace(2,"swapsolstat: file open error path=%s\n",path); + return; + } + trace(3,"swapsolstat: path=%s\n",path); +} + + +/* output solution status ----------------------------------------------------*/ +void outsolstat(rtk_t *rtk) +{ + ssat_t *ssat; + double tow; + char buff[MAXSOLMSG+1],id[32]; + int i,j,n,week,nfreq,nf=NF_RTK(&rtk->opt); + + if (statlevel<=0||!fp_stat||!rtk->sol.stat) return; + + trace(3,"outsolstat:\n"); + + /* swap solution status file */ + swapsolstat(); + + /* write solution status */ + n=rtkoutstat(rtk,buff); buff[n]='\0'; + + fputs(buff,fp_stat); + + if (rtk->sol.stat==SOLQ_NONE||statlevel<=1) return; + + tow=time2gpst(rtk->sol.time,&week); + nfreq=rtk->opt.mode>=PMODE_DGPS?nf:1; + + /* write residuals and status */ + for (i=0;issat+i; + if (!ssat->vs) continue; + satno2id(i+1,id); + for (j=0;jazel[0]*R2D,ssat->azel[1]*R2D, + ssat->resp[j],ssat->resc[j],ssat->vsat[j],ssat->snr[j]*0.25, + ssat->fix[j],ssat->slip[j]&3,ssat->lock[j],ssat->outc[j], + ssat->slipc[j],ssat->rejc[j]); + } + } +} + + +/* save error message --------------------------------------------------------*/ +void errmsg(rtk_t *rtk, const char *format, ...) +{ + char buff[256],tstr[32]; + int n; + va_list ap; + time2str(rtk->sol.time,tstr,2); + n=sprintf(buff,"%s: ",tstr+11); + va_start(ap,format); + n+=vsprintf(buff+n,format,ap); + va_end(ap); + n=nneb?n:MAXERRMSG-rtk->neb; + memcpy(rtk->errbuf+rtk->neb,buff,n); + rtk->neb+=n; + trace(2,"%s",buff); +} + + +/* single-differenced observable ---------------------------------------------*/ +double sdobs(const obsd_t *obs, int i, int j, int f) +{ + double pi=ferr[3]*bl/1E4,d=SPEED_OF_LIGHT*opt->sclkstab*dt,fact=1.0; + double sinel=sin(el); + int i=sys==SYS_GLO?1:(sys==SYS_GAL?2:0),nf=NF_RTK(opt); + + /* extended error model */ + if (f>=nf&&opt->exterr.ena[0]) + { /* code */ + a=opt->exterr.cerr[i][ (f-nf)*2]; + b=opt->exterr.cerr[i][1+(f-nf)*2]; + if (sys==SYS_SBS) {a*=EFACT_SBS; b*=EFACT_SBS;} + } + else if (fexterr.ena[1]) + { /* phase */ + a=opt->exterr.perr[i][ f*2]; + b=opt->exterr.perr[i][1+f*2]; + if (sys==SYS_SBS) {a*=EFACT_SBS; b*=EFACT_SBS;} + } + else + { /* normal error model */ + if (f>=nf) fact=opt->eratio[f-nf]; + if (fact<=0.0) fact=opt->eratio[0]; + fact*=sys==SYS_GLO?EFACT_GLO:(sys==SYS_SBS?EFACT_SBS:EFACT_GPS); + a=fact*opt->err[1]; + b=fact*opt->err[2]; + } + return 2.0*(opt->ionoopt==IONOOPT_IFLC?3.0:1.0)*(a*a+b*b/sinel/sinel+c*c)+d*d; +} + + +/* baseline length -----------------------------------------------------------*/ +double baseline(const double *ru, const double *rb, double *dr) { int i; - for (i = 0; in; i++) - { - if (nav->eph[i].sat!=sat) continue; - return SPEED_OF_LIGHT*nav->eph[i].tgd[0]; - } - return 0.0; + for (i=0;i<3;i++) dr[i]=ru[i]-rb[i]; + return norm(dr,3); } -/* psendorange with code bias correction -------------------------------------*/ - double prange(const obsd_t *obs, const nav_t *nav, const double *azel, - int iter, const prcopt_t *opt, double *var) +/* initialize state and covariance -------------------------------------------*/ +void initx(rtk_t *rtk, double xi, double var, int i) { - const double *lam = nav->lam[obs->sat-1]; - double PC, P1, P2, P1_P2, P1_C1, P2_C2, gamma; - int i = 0, j = 1, sys; - - *var = 0.0; - - if (!(sys = satsys(obs->sat, NULL))) return 0.0; - - /* L1-L2 for GPS/GLO/QZS, L1-L5 for GAL/SBS */ - if (NFREQ >= 3 && (sys & (SYS_GAL | SYS_SBS))) j = 2; - - if (NFREQ<2 || lam[i] == 0.0 || lam[j] == 0.0) return 0.0; - - /* test snr mask */ - if (iter>0) - { - if (testsnr(0, i, azel[1], obs->SNR[i]*0.25, &opt->snrmask)) + int j; + rtk->x[i]=xi; + for (j=0;jnx;j++) { - trace(4, "snr mask: %s sat=%2d el=%.1f snr=%.1f\n", - time_str(obs->time, 0), obs->sat, azel[1]*R2D, obs->SNR[i]*0.25); - return 0.0; + rtk->P[i+j*rtk->nx]=rtk->P[j+i*rtk->nx]=i==j?var:0.0; } - if (opt->ionoopt == IONOOPT_IFLC) - { - if (testsnr(0, j, azel[1], obs->SNR[j]*0.25, &opt->snrmask)) return 0.0; - } - } - gamma = std::pow(lam[j], 2.0) / std::pow(lam[i], 2.0); /* f1^2/f2^2 */ - P1 = obs->P[i]; - P2 = obs->P[j]; - P1_P2 = nav->cbias[obs->sat-1][0]; - P1_C1 = nav->cbias[obs->sat-1][1]; - P2_C2 = nav->cbias[obs->sat-1][2]; - - /* if no P1-P2 DCB, use TGD instead */ - if (P1_P2 == 0.0 && (sys&(SYS_GPS|SYS_GAL|SYS_QZS))) - { - P1_P2 = (1.0-gamma)*gettgd(obs->sat, nav); - } - if (opt->ionoopt == IONOOPT_IFLC) - { /* dual-frequency */ - - if (P1 == 0.0 || P2 == 0.0) return 0.0; - if (obs->code[i] == CODE_L1C) P1 += P1_C1; /* C1->P1 */ - if (obs->code[j] == CODE_L2C) P2 += P2_C2; /* C2->P2 */ - - /* iono-free combination */ - PC = (gamma*P1-P2) / (gamma-1.0); - } - else - { /* single-frequency */ - if (P1 == 0.0) return 0.0; - if (obs->code[i] == CODE_L1C) P1 += P1_C1; /* C1->P1 */ - PC = P1-P1_P2 / (1.0-gamma); - } - if (opt->sateph == EPHOPT_SBAS) PC -= P1_C1; /* sbas clock based C1 */ - - *var = std::pow(ERR_CBIAS, 2.0); - - return PC; } -/* ionospheric correction ------------------------------------------------------ -* compute ionospheric correction -* args : gtime_t time I time -* nav_t *nav I navigation data -* int sat I satellite number -* double *pos I receiver position {lat,lon,h} (rad|m) -* double *azel I azimuth/elevation angle {az,el} (rad) -* int ionoopt I ionospheric correction option (IONOOPT_???) -* double *ion O ionospheric delay (L1) (m) -* double *var O ionospheric delay (L1) variance (m^2) -* return : status(1:ok,0:error) -*-----------------------------------------------------------------------------*/ -int ionocorr(gtime_t time, const nav_t *nav, int sat, const double *pos, - const double *azel, int ionoopt, double *ion, double *var) +/* select common satellites between rover and reference station --------------*/ +int selsat(const obsd_t *obs, double *azel, int nu, int nr, + const prcopt_t *opt, int *sat, int *iu, int *ir) { - trace(4, "ionocorr: time=%s opt=%d sat=%2d pos=%.3f %.3f azel=%.3f %.3f\n", - time_str(time, 3), ionoopt, sat, pos[0]*R2D, pos[1]*R2D, azel[0]*R2D, - azel[1]*R2D); + int i,j,k=0; - /* broadcast model */ - if (ionoopt == IONOOPT_BRDC) - { - *ion = ionmodel(time, nav->ion_gps, pos, azel); - *var = std::pow(*ion*ERR_BRDCI, 2.0); - return 1; - } - /* sbas ionosphere model */ - if (ionoopt == IONOOPT_SBAS) - { - return sbsioncorr(time, nav, pos, azel, ion, var); - } - /* ionex tec model */ - if (ionoopt == IONOOPT_TEC) - { - return iontec(time, nav, pos, azel, 1, ion, var); - } - /* qzss broadcast model */ - if (ionoopt == IONOOPT_QZS && norm(nav->ion_qzs, 8)>0.0) - { - *ion = ionmodel(time, nav->ion_qzs, pos, azel); - *var = std::pow(*ion*ERR_BRDCI, 2.0); - return 1; - } - /* lex ionosphere model */ - if (ionoopt == IONOOPT_LEX) - { - return lexioncorr(time, nav, pos, azel, ion, var); - } - *ion = 0.0; - *var = ionoopt == IONOOPT_OFF ? std::pow(ERR_ION, 2.0) : 0.0; - return 1; -} + trace(3,"selsat : nu=%d nr=%d\n",nu,nr); -/* tropospheric correction ----------------------------------------------------- -* compute tropospheric correction -* args : gtime_t time I time -* nav_t *nav I navigation data -* double *pos I receiver position {lat,lon,h} (rad|m) -* double *azel I azimuth/elevation angle {az,el} (rad) -* int tropopt I tropospheric correction option (TROPOPT_???) -* double *trp O tropospheric delay (m) -* double *var O tropospheric delay variance (m^2) -* return : status(1:ok,0:error) -*-----------------------------------------------------------------------------*/ -int tropcorr(gtime_t time, const nav_t *nav, const double *pos, - const double *azel, int tropopt, double *trp, double *var) -{ - trace(4, "tropcorr: time=%s opt=%d pos=%.3f %.3f azel=%.3f %.3f\n", - time_str(time, 3), tropopt, pos[0]*R2D, pos[1]*R2D, azel[0]*R2D, - azel[1]*R2D); - - /* saastamoinen model */ - if (tropopt == TROPOPT_SAAS || tropopt == TROPOPT_EST || tropopt == TROPOPT_ESTG) - { - *trp = tropmodel(time, pos, azel, REL_HUMI); - *var = std::pow(ERR_SAAS / (sin(azel[1])+0.1), 2.0); - return 1; - } - /* sbas troposphere model */ - if (tropopt == TROPOPT_SBAS) - { - *trp = sbstropcorr(time, pos, azel, var); - return 1; - } - /* no correction */ - *trp = 0.0; - *var = tropopt == TROPOPT_OFF ? std::pow(ERR_TROP, 2.0) : 0.0; - return 1; -} - - -/* pseudorange residuals -----------------------------------------------------*/ -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) -{ - double r, dion, dtrp, vmeas, vion, vtrp, rr[3], pos[3], dtr, e[3], P, lam_L1; - int i, j, nv = 0, sys, mask[4] = {0}; - - trace(3, "resprng : n=%d\n", n); - - for (i = 0; i<3; i++) rr[i] = x[i]; dtr = x[3]; - - ecef2pos(rr, pos); - - for (i=*ns = 0; ielmin) continue; - - /* psudorange with code bias correction */ - if ((P = prange(obs+i, nav, azel+i*2, iter, opt, &vmeas)) == 0.0) continue; - - /* excluded satellite? */ - if (satexclude(obs[i].sat, svh[i], opt)) continue; - - /* ionospheric corrections */ - if (!ionocorr(obs[i].time, nav, obs[i].sat, pos, azel+i*2, - iter>0 ? opt->ionoopt : IONOOPT_BRDC, &dion, &vion)) continue; - - /* GPS-L1 -> L1/B1 */ - if ((lam_L1 = nav->lam[obs[i].sat-1][0])>0.0) - { - dion *= std::pow(lam_L1/lam_carr[0], 2.0); - } - /* tropospheric corrections */ - if (!tropcorr(obs[i].time, nav, pos, azel+i*2, - iter>0 ? opt->tropopt : TROPOPT_SAAS, &dtrp, &vtrp)) - { - continue; + if (obs[i].satobs[j].sat) i--; + else if (azel[1+j*2]>=opt->elmin) { /* elevation at base station */ + sat[k]=obs[i].sat; iu[k]=i; ir[k++]=j; + trace(4,"(%2d) sat=%3d iu=%2d ir=%2d\n",k-1,obs[i].sat,i,j); } - /* pseudorange residual */ - v[nv] = P-(r+dtr-SPEED_OF_LIGHT*dts[i*2]+dion+dtrp); + } + return k; +} - /* design matrix */ - for (j = 0; jopt.mode==PMODE_FIXED) + { + for (i=0;i<3;i++) initx(rtk,rtk->opt.ru[i],1E-8,i); + return; + } + /* initialize position for first epoch */ + if (norm(rtk->x,3)<=0.0) + { + for (i=0;i<3;i++) initx(rtk,rtk->sol.rr[i],VAR_POS,i); + if (rtk->opt.dynamics) + { + for (i=3;i<6;i++) initx(rtk,rtk->sol.rr[i],VAR_VEL,i); + for (i=6;i<9;i++) initx(rtk,1E-6,VAR_ACC,i); + } + } + /* static mode */ + if (rtk->opt.mode==PMODE_STATIC) return; - trace(4, "sat=%2d azel=%5.1f %4.1f res=%7.3f sig=%5.3f\n", obs[i].sat, - azel[i*2]*R2D, azel[1+i*2]*R2D, resp[i], sqrt(var[nv-1])); - } - /* constraint to avoid rank-deficient */ - for (i = 0; i<4; i++) + /* kinmatic mode without dynamics */ + if (!rtk->opt.dynamics) + { + for (i=0;i<3;i++) initx(rtk,rtk->sol.rr[i],VAR_POS,i); + return; + } + /* check variance of estimated postion */ + for (i=0;i<3;i++) var+=rtk->P[i+i*rtk->nx]; var/=3.0; + + if (var>VAR_POS) + { + /* reset position with large variance */ + for (i=0;i<3;i++) initx(rtk,rtk->sol.rr[i],VAR_POS,i); + for (i=3;i<6;i++) initx(rtk,rtk->sol.rr[i],VAR_VEL,i); + for (i=6;i<9;i++) initx(rtk,1E-6,VAR_ACC,i); + trace(2,"reset rtk position due to large variance: var=%.3f\n",var); + return; + } + /* state transition of position/velocity/acceleration */ + F=eye(rtk->nx); FP=mat(rtk->nx,rtk->nx); xp=mat(rtk->nx,1); + + for (i=0;i<6;i++) + { + F[i+(i+3)*rtk->nx]=tt; + } + /* x=F*x, P=F*P*F+Q */ + matmul("NN",rtk->nx,1,rtk->nx,1.0,F,rtk->x,0.0,xp); + matcpy(rtk->x,xp,rtk->nx,1); + matmul("NN",rtk->nx,rtk->nx,rtk->nx,1.0,F,rtk->P,0.0,FP); + matmul("NT",rtk->nx,rtk->nx,rtk->nx,1.0,FP,F,0.0,rtk->P); + + /* process noise added to only acceleration */ + Q[0]=Q[4]=std::pow(rtk->opt.prn[3], 2.0); Q[8]=std::pow(rtk->opt.prn[4], 2.0); + ecef2pos(rtk->x,pos); + covecef(pos,Q,Qv); + for (i=0;i<3;i++) for (j=0;j<3;j++) + { + rtk->P[i+6+(j+6)*rtk->nx]+=Qv[i+j*3]; + } + free(F); free(FP); free(xp); +} + + +/* temporal update of ionospheric parameters ---------------------------------*/ +void udion(rtk_t *rtk, double tt, double bl, const int *sat, int ns) +{ + double el,fact; + int i,j; + + trace(3,"udion : tt=%.1f bl=%.0f ns=%d\n",tt,bl,ns); + + for (i=1;i<=MAXSAT;i++) + { + j=II_RTK(i,&rtk->opt); + if (rtk->x[j]!=0.0&& + rtk->ssat[i-1].outc[0]>GAP_RESION&&rtk->ssat[i-1].outc[1]>GAP_RESION) + rtk->x[j]=0.0; + } + for (i=0;iopt); + + if (rtk->x[j]==0.0) + { + initx(rtk,1E-6,std::pow(rtk->opt.std[1]*bl/1e4, 2.0),j); + } + else + { + /* elevation dependent factor of process noise */ + el=rtk->ssat[sat[i]-1].azel[1]; + fact=cos(el); + rtk->P[j+j*rtk->nx]+=std::pow(rtk->opt.prn[1]*bl/1e4*fact, 2.0)*tt; + } + } +} + + +/* temporal update of tropospheric parameters --------------------------------*/ +void udtrop(rtk_t *rtk, double tt, double bl) +{ + int i,j,k; + + trace(3,"udtrop : tt=%.1f\n",tt); + + for (i=0;i<2;i++) + { + j=IT_RTK(i,&rtk->opt); + + if (rtk->x[j]==0.0) + { + initx(rtk,INIT_ZWD,std::pow(rtk->opt.std[2], 2.0),j); /* initial zwd */ + + if (rtk->opt.tropopt>=TROPOPT_ESTG) + { + for (k=0;k<2;k++) initx(rtk,1E-6,VAR_GRA,++j); + } + } + else + { + rtk->P[j+j*rtk->nx]+=std::pow(rtk->opt.prn[2], 2.0)*tt; + + if (rtk->opt.tropopt>=TROPOPT_ESTG) + { + for (k=0;k<2;k++) + { + rtk->P[++j*(1+rtk->nx)]+=std::pow(rtk->opt.prn[2]*0.3, 2.0)*fabs(rtk->tt); + } + } + } + } +} + + +/* temporal update of receiver h/w biases ------------------------------------*/ +void udrcvbias(rtk_t *rtk, double tt) +{ + int i,j; + + trace(3,"udrcvbias: tt=%.1f\n",tt); + + for (i=0;iopt); + + if (rtk->x[j]==0.0) + { + initx(rtk,1E-6,VAR_HWBIAS,j); + } + /* hold to fixed solution */ + else if (rtk->nfix>=rtk->opt.minfix&&rtk->sol.ratio>rtk->opt.thresar[0]) + { + initx(rtk,rtk->xa[j],rtk->Pa[j+j*rtk->na],j); + } + else + { + rtk->P[j+j*rtk->nx]+=std::pow(PRN_HWBIAS, 2.0)*tt; + } + } +} + + +/* detect cycle slip by LLI --------------------------------------------------*/ +void detslp_ll(rtk_t *rtk, const obsd_t *obs, int i, int rcv) +{ + unsigned int slip,LLI; + int f,sat=obs[i].sat; + + trace(3,"detslp_ll: i=%d rcv=%d\n",i,rcv); + + for (f=0;fopt.nf;f++) + { + + if (obs[i].L[f]==0.0) continue; + + /* restore previous LLI */ + if (rcv==1) LLI=getbitu(&rtk->ssat[sat-1].slip[f],0,2); /* rover */ + else LLI=getbitu(&rtk->ssat[sat-1].slip[f],2,2); /* base */ + + /* detect slip by cycle slip flag in LLI */ + if (rtk->tt>=0.0) { /* forward */ + if (obs[i].LLI[f]&1) + { + errmsg(rtk,"slip detected forward (sat=%2d rcv=%d F=%d LLI=%x)\n", + sat,rcv,f+1,obs[i].LLI[f]); + } + slip=obs[i].LLI[f]; + } + else + { /* backward */ + if (LLI&1) + { + errmsg(rtk,"slip detected backward (sat=%2d rcv=%d F=%d LLI=%x)\n", + sat,rcv,f+1,LLI); + } + slip=LLI; + } + /* detect slip by parity unknown flag transition in LLI */ + if (((LLI&2)&&!(obs[i].LLI[f]&2))||(!(LLI&2)&&(obs[i].LLI[f]&2))) + { + errmsg(rtk,"slip detected half-cyc (sat=%2d rcv=%d F=%d LLI=%x->%x)\n", + sat,rcv,f+1,LLI,obs[i].LLI[f]); + slip|=1; + } + /* save current LLI */ + if (rcv==1) setbitu(&rtk->ssat[sat-1].slip[f],0,2,obs[i].LLI[f]); + else setbitu(&rtk->ssat[sat-1].slip[f],2,2,obs[i].LLI[f]); + + /* save slip and half-cycle valid flag */ + rtk->ssat[sat-1].slip[f]|=(unsigned char)slip; + rtk->ssat[sat-1].half[f]=(obs[i].LLI[f]&2)?0:1; + } +} + + +/* detect cycle slip by L1-L2 geometry free phase jump -----------------------*/ +void detslp_gf_L1L2(rtk_t *rtk, const obsd_t *obs, int i, int j, + const nav_t *nav) +{ + int sat=obs[i].sat; + double g0,g1; + + trace(3,"detslp_gf_L1L2: i=%d j=%d\n",i,j); + + if (rtk->opt.nf<=1||(g1=gfobs_L1L2(obs,i,j,nav->lam[sat-1]))==0.0) return; + + g0=rtk->ssat[sat-1].gf; rtk->ssat[sat-1].gf=g1; + + if (g0!=0.0&&fabs(g1-g0)>rtk->opt.thresslip) + { + + rtk->ssat[sat-1].slip[0]|=1; + rtk->ssat[sat-1].slip[1]|=1; + + errmsg(rtk,"slip detected (sat=%2d GF_L1_L2=%.3f %.3f)\n",sat,g0,g1); + } +} + + +/* detect cycle slip by L1-L5 geometry free phase jump -----------------------*/ +void detslp_gf_L1L5(rtk_t *rtk, const obsd_t *obs, int i, int j, + const nav_t *nav) +{ + int sat=obs[i].sat; + double g0,g1; + + trace(3,"detslp_gf_L1L5: i=%d j=%d\n",i,j); + + if (rtk->opt.nf<=2||(g1=gfobs_L1L5(obs,i,j,nav->lam[sat-1]))==0.0) return; + + g0=rtk->ssat[sat-1].gf2; rtk->ssat[sat-1].gf2=g1; + + if (g0!=0.0&&fabs(g1-g0)>rtk->opt.thresslip) + { + rtk->ssat[sat-1].slip[0]|=1; + rtk->ssat[sat-1].slip[2]|=1; + + errmsg(rtk,"slip detected (sat=%2d GF_L1_L5=%.3f %.3f)\n",sat,g0,g1); + } +} + + +/* detect cycle slip by doppler and phase difference -------------------------*/ +void detslp_dop(rtk_t *rtk, const obsd_t *obs, int i, int rcv, + const nav_t *nav) +{ + /* detection with doppler disabled because of clock-jump issue (v.2.3.0) */ +#if 0 + int f,sat=obs[i].sat; + double tt,dph,dpt,lam,thres; + + trace(3,"detslp_dop: i=%d rcv=%d\n",i,rcv); + + for (f=0;fopt.nf;f++) + { + if (obs[i].L[f]==0.0||obs[i].D[f]==0.0||rtk->ph[rcv-1][sat-1][f]==0.0) + { + continue; + } + if (fabs(tt=timediff(obs[i].time,rtk->pt[rcv-1][sat-1][f]))lam[sat-1][f])<=0.0) continue; + + /* cycle slip threshold (cycle) */ + thres=MAXACC*tt*tt/2.0/lam+rtk->opt.err[4]*fabs(tt)*4.0; + + /* phase difference and doppler x time (cycle) */ + dph=obs[i].L[f]-rtk->ph[rcv-1][sat-1][f]; + dpt=-obs[i].D[f]*tt; + + if (fabs(dph-dpt)<=thres) continue; + + rtk->slip[sat-1][f]|=1; + + errmsg(rtk,"slip detected (sat=%2d rcv=%d L%d=%.3f %.3f thres=%.3f)\n", + sat,rcv,f+1,dph,dpt,thres); + } +#endif +} + + +/* temporal update of phase biases -------------------------------------------*/ +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) +{ + double cp,pr,cp1,cp2,pr1,pr2,*bias,offset,lami,lam1,lam2,C1,C2; + int i,j,f,slip,reset,nf=NF_RTK(&rtk->opt); + + trace(3,"udbias : tt=%.1f ns=%d\n",tt,ns); + + for (i=0;iopt.nf;f++) rtk->ssat[sat[i]-1].slip[f]&=0xFC; + detslp_ll(rtk,obs,iu[i],1); + detslp_ll(rtk,obs,ir[i],2); + + /* detect cycle slip by geometry-free phase jump */ + detslp_gf_L1L2(rtk,obs,iu[i],ir[i],nav); + detslp_gf_L1L5(rtk,obs,iu[i],ir[i],nav); + + /* detect cycle slip by doppler and phase difference */ + detslp_dop(rtk,obs,iu[i],1,nav); + detslp_dop(rtk,obs,ir[i],2,nav); + + /* update half-cycle valid flag */ + for (f=0;fssat[sat[i]-1].half[f]= + !((obs[iu[i]].LLI[f]&2)||(obs[ir[i]].LLI[f]&2)); + } + } + for (f=0;fssat[i-1].outc[f]>(unsigned int)rtk->opt.maxout; + + if (rtk->opt.modear==ARMODE_INST&&rtk->x[IB_RTK(i,f,&rtk->opt)]!=0.0) + { + initx(rtk,0.0,0.0,IB_RTK(i,f,&rtk->opt)); + } + else if (reset&&rtk->x[IB_RTK(i,f,&rtk->opt)]!=0.0) + { + initx(rtk,0.0,0.0,IB_RTK(i,f,&rtk->opt)); + trace(3,"udbias : obs outage counter overflow (sat=%3d L%d n=%d)\n", + i,f+1,rtk->ssat[i-1].outc[f]); + } + if (rtk->opt.modear!=ARMODE_INST&&reset) + { + rtk->ssat[i-1].lock[f]=-rtk->opt.minlock; + } + } + /* reset phase-bias if detecting cycle slip */ + for (i=0;iopt); + rtk->P[j+j*rtk->nx]+=rtk->opt.prn[0]*rtk->opt.prn[0]*tt; + slip=rtk->ssat[sat[i]-1].slip[f]; + if (rtk->opt.ionoopt==IONOOPT_IFLC) slip|=rtk->ssat[sat[i]-1].slip[1]; + if (rtk->opt.modear==ARMODE_INST||!(slip&1)) continue; + rtk->x[j]=0.0; + rtk->ssat[sat[i]-1].lock[f]=-rtk->opt.minlock; + } + bias=zeros(ns,1); + + /* estimate approximate phase-bias by phase - code */ + for (i=j=0,offset=0.0;iopt.ionoopt!=IONOOPT_IFLC) + { + cp=sdobs(obs,iu[i],ir[i],f); /* cycle */ + pr=sdobs(obs,iu[i],ir[i],f+NFREQ); + lami=nav->lam[sat[i]-1][f]; + if (cp==0.0||pr==0.0||lami<=0.0) continue; + + bias[i]=cp-pr/lami; + } + else + { + cp1=sdobs(obs,iu[i],ir[i],0); + cp2=sdobs(obs,iu[i],ir[i],1); + pr1=sdobs(obs,iu[i],ir[i],NFREQ); + pr2=sdobs(obs,iu[i],ir[i],NFREQ+1); + lam1=nav->lam[sat[i]-1][0]; + lam2=nav->lam[sat[i]-1][1]; + if (cp1==0.0||cp2==0.0||pr1==0.0||pr2==0.0||lam1<=0.0||lam2<=0.0) continue; + + C1= std::pow(lam2, 2.0)/(std::pow(lam2, 2.0)-std::pow(lam1, 2.0)); + C2=-std::pow(lam1, 2.0)/(std::pow(lam2, 2.0)-std::pow(lam1, 2.0)); + bias[i]=(C1*lam1*cp1+C2*lam2*cp2)-(C1*pr1+C2*pr2); + } + if (rtk->x[IB_RTK(sat[i],f,&rtk->opt)]!=0.0) + { + offset+=bias[i]-rtk->x[IB_RTK(sat[i],f,&rtk->opt)]; + j++; + } + } + /* correct phase-bias offset to enssure phase-code coherency */ + if (j>0) { + for (i=1;i<=MAXSAT;i++) + { + if (rtk->x[IB_RTK(i,f,&rtk->opt)]!=0.0) rtk->x[IB_RTK(i,f,&rtk->opt)]+=offset/j; + } + } + /* set initial states of phase-bias */ + for (i=0;ix[IB_RTK(sat[i],f,&rtk->opt)]!=0.0) continue; + initx(rtk,bias[i],std::pow(rtk->opt.std[0], 2.0),IB_RTK(sat[i],f,&rtk->opt)); + } + free(bias); + } +} + + +/* temporal update of states --------------------------------------------------*/ +void udstate(rtk_t *rtk, const obsd_t *obs, const int *sat, + const int *iu, const int *ir, int ns, const nav_t *nav) +{ + double tt=fabs(rtk->tt),bl,dr[3]; + + trace(3,"udstate : ns=%d\n",ns); + + /* temporal update of position/velocity/acceleration */ + udpos(rtk,tt); + + /* temporal update of ionospheric parameters */ + if (rtk->opt.ionoopt>=IONOOPT_EST) + { + bl=baseline(rtk->x,rtk->rb,dr); + udion(rtk,tt,bl,sat,ns); + } + /* temporal update of tropospheric parameters */ + if (rtk->opt.tropopt>=TROPOPT_EST) + { + udtrop(rtk,tt,bl); + } + /* temporal update of eceiver h/w bias */ + if (rtk->opt.glomodear==2&&(rtk->opt.navsys&SYS_GLO)) + { + udrcvbias(rtk,tt); + } + /* temporal update of phase-bias */ + if (rtk->opt.mode>PMODE_DGPS) + { + udbias(rtk,tt,obs,sat,iu,ir,ns,nav); + } +} + + +/* undifferenced phase/code residual for satellite ---------------------------*/ +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) +{ + const double *lam=nav->lam[obs->sat-1]; + double f1,f2,C1,C2,dant_if; + int i,nf=NF_RTK(opt); + + if (opt->ionoopt==IONOOPT_IFLC) + { /* iono-free linear combination */ + if (lam[0]==0.0||lam[1]==0.0) return; + + if (testsnr(base,0,azel[1],obs->SNR[0]*0.25,&opt->snrmask)|| + testsnr(base,1,azel[1],obs->SNR[1]*0.25,&opt->snrmask)) return; + + f1=SPEED_OF_LIGHT/lam[0]; + f2=SPEED_OF_LIGHT/lam[1]; + C1= std::pow(f1, 2.0)/(std::pow(f1, 2.0)-std::pow(f2, 2.0)); + C2=-std::pow(f2, 2.0)/(std::pow(f1, 2.0)-std::pow(f2, 2.0)); + dant_if=C1*dant[0]+C2*dant[1]; + + if (obs->L[0]!=0.0&&obs->L[1]!=0.0) + { + y[0]=C1*obs->L[0]*lam[0]+C2*obs->L[1]*lam[1]-r-dant_if; + } + if (obs->P[0]!=0.0&&obs->P[1]!=0.0) + { + y[1]=C1*obs->P[0]+C2*obs->P[1]-r-dant_if; + } + } + else + { + for (i=0;iSNR[i]*0.25,&opt->snrmask)) + { + continue; + } + /* residuals = observable - pseudorange */ + if (obs->L[i]!=0.0) y[i ]=obs->L[i]*lam[i]-r-dant[i]; + if (obs->P[i]!=0.0) y[i+nf]=obs->P[i] -r-dant[i]; + } + } +} + + +/* undifferenced phase/code residuals ----------------------------------------*/ +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) +{ + double r,rr_[3],pos[3],dant[NFREQ]={0},disp[3]; + double zhd,zazel[]={0.0,90.0*D2R}; + int i,nf=NF_RTK(opt); + + trace(3,"zdres : n=%d\n",n); + + for (i=0;itidecorr) + { + tidedisp(gpst2utc(obs[0].time),rr_,opt->tidecorr,&nav->erp, + opt->odisp[base],disp); + for (i=0;i<3;i++) rr_[i]+=disp[i]; + } + ecef2pos(rr_,pos); + + for (i=0;ielmin) continue; + + /* excluded satellite? */ + if (satexclude(obs[i].sat,svh[i],opt)) continue; + + /* satellite clock-bias */ + r+=-SPEED_OF_LIGHT*dts[i*2]; + + /* troposphere delay model (hydrostatic) */ + zhd=tropmodel(obs[0].time,pos,zazel,0.0); + r+=tropmapf(obs[i].time,pos,azel+i*2,NULL)*zhd; + + /* receiver antenna phase center correction */ + antmodel(opt->pcvr+index,opt->antdel[index],azel+i*2,opt->posopt[1], + dant); + + /* undifferenced phase/code residual for satellite */ + zdres_sat(base,r,obs+i,nav,azel+i*2,dant,opt,y+i*nf*2); + } + trace(4,"rr_=%.3f %.3f %.3f\n",rr_[0],rr_[1],rr_[2]); + trace(4,"pos=%.9f %.9f %.3f\n",pos[0]*R2D,pos[1]*R2D,pos[2]); + for (i=0;iopt.baseline[0]<=0.0) return 0; + + /* time-adjusted baseline vector and length */ + for (i=0;i<3;i++) + { + xb[i]=rtk->rb[i]+rtk->rb[i+3]*rtk->sol.age; + b[i]=x[i]-xb[i]; + } + bb=norm(b,3); + + /* approximate variance of solution */ + if (P) + { + for (i=0;i<3;i++) var+=P[i+i*rtk->nx]; + var/=3.0; + } + /* check nonlinearity */ + if (var>thres*thres*bb*bb) + { + trace(3,"constbl : equation nonlinear (bb=%.3f var=%.3f)\n",bb,var); + return 0; + } + /* constraint to baseline length */ + v[index]=rtk->opt.baseline[0]-bb; + if (H) + { + for (i=0;i<3;i++) H[i+index*rtk->nx]=b[i]/bb; + } + Ri[index]=0.0; + Rj[index]=std::pow(rtk->opt.baseline[1], 2.0); + + trace(4,"baseline len v=%13.3f R=%8.6f %8.6f\n",v[index],Ri[index],Rj[index]); + + return 1; +} + + +/* precise tropspheric model -------------------------------------------------*/ +double prectrop(gtime_t time, const double *pos, int r, + const double *azel, const prcopt_t *opt, const double *x, + double *dtdx) +{ + double m_w=0.0,cotz,grad_n,grad_e; + int i=IT_RTK(r,opt); + + /* wet mapping function */ + tropmapf(time,pos,azel,&m_w); + + if (opt->tropopt>=TROPOPT_ESTG&&azel[1]>0.0) + { + /* m_w=m_0+m_0*cot(el)*(Gn*cos(az)+Ge*sin(az)): ref [6] */ + cotz=1.0/tan(azel[1]); + grad_n=m_w*cotz*cos(azel[0]); + grad_e=m_w*cotz*sin(azel[0]); + m_w+=grad_n*x[i+1]+grad_e*x[i+2]; + dtdx[1]=grad_n*x[i]; + dtdx[2]=grad_e*x[i]; + } + else dtdx[1]=dtdx[2]=0.0; + dtdx[0]=m_w; + return m_w*x[i]; +} + + +/* glonass inter-channel bias correction -------------------------------------*/ +double gloicbcorr(int sat1, int sat2, const prcopt_t *opt, double lam1, + double lam2, int f) +{ + double dfreq; + + if (f>=NFREQGLO||f>=opt->nf||!opt->exterr.ena[2]) return 0.0; + + dfreq=(SPEED_OF_LIGHT/lam1-SPEED_OF_LIGHT/lam2)/(f==0?DFRQ1_GLO:DFRQ2_GLO); + + return opt->exterr.gloicb[f]*0.01*dfreq; /* (m) */ +} + + +/* test navi system (m=0:gps/qzs/sbs,1:glo,2:gal,3:bds) ----------------------*/ +int test_sys(int sys, int m) +{ + switch (sys) { - if (mask[i]) continue; - v[nv] = 0.0; - for (j = 0; jopt; + double bl,dr[3],posu[3],posr[3],didxi=0.0,didxj=0.0,*im; + double *tropr,*tropu,*dtdxr,*dtdxu,*Ri,*Rj,lami,lamj,fi,fj,df,*Hi=NULL; + int i,j,k,m,f,ff,nv=0,nb[NFREQ*4*2+2]={0},b=0,sysi,sysj,nf=NF_RTK(opt); + + trace(3,"ddres : dt=%.1f nx=%d ns=%d\n",dt,rtk->nx,ns); + + bl=baseline(x,rtk->rb,dr); + ecef2pos(x,posu); ecef2pos(rtk->rb,posr); + + Ri=mat(ns*nf*2+2,1); Rj=mat(ns*nf*2+2,1); im=mat(ns,1); + tropu=mat(ns,1); tropr=mat(ns,1); dtdxu=mat(ns,3); dtdxr=mat(ns,3); + + for (i=0;issat[i].resp[j]=rtk->ssat[i].resc[j]=0.0; + } + /* compute factors of ionospheric and tropospheric delay */ + for (i=0;iionoopt>=IONOOPT_EST) + { + im[i]=(ionmapf(posu,azel+iu[i]*2)+ionmapf(posr,azel+ir[i]*2))/2.0; + } + if (opt->tropopt>=TROPOPT_EST) + { + tropu[i]=prectrop(rtk->sol.time,posu,0,azel+iu[i]*2,opt,x,dtdxu+i*3); + tropr[i]=prectrop(rtk->sol.time,posr,1,azel+ir[i]*2,opt,x,dtdxr+i*3); + } + } + for (m=0;m<4;m++) /* m=0:gps/qzs/sbs,1:glo,2:gal,3:bds */ + + for (f=opt->mode>PMODE_DGPS?0:nf;fssat[sat[j]-1].sys; + if (!test_sys(sysi,m)) continue; + if (!validobs(iu[j],ir[j],f,nf,y)) continue; + if (i<0||azel[1+iu[j]*2]>=azel[1+iu[i]*2]) i=j; + } + if (i<0) continue; + + /* make double difference */ + for (j=0;jssat[sat[i]-1].sys; + sysj=rtk->ssat[sat[j]-1].sys; + if (!test_sys(sysj,m)) continue; + if (!validobs(iu[j],ir[j],f,nf,y)) continue; + + ff=f%nf; + lami=nav->lam[sat[i]-1][ff]; + lamj=nav->lam[sat[j]-1][ff]; + if (lami<=0.0||lamj<=0.0) continue; + if (H) + { + Hi=H+nv*rtk->nx; + for (k=0;knx;k++) Hi[k]=0.0; + } + /* double-differenced residual */ + v[nv]=(y[f+iu[i]*nf*2]-y[f+ir[i]*nf*2])- + (y[f+iu[j]*nf*2]-y[f+ir[j]*nf*2]); + + /* partial derivatives by rover position */ + if (H) + { + for (k=0;k<3;k++) + { + Hi[k]=-e[k+iu[i]*3]+e[k+iu[j]*3]; + } + } + /* double-differenced ionospheric delay term */ + if (opt->ionoopt==IONOOPT_EST) + { + fi=lami/lam_carr[0]; fj=lamj/lam_carr[0]; + didxi=(ftropopt==TROPOPT_EST||opt->tropopt==TROPOPT_ESTG) + { + v[nv]-=(tropu[i]-tropu[j])-(tropr[i]-tropr[j]); + for (k=0;k<(opt->tropoptionoopt!=IONOOPT_IFLC) + { + v[nv]-=lami*x[IB_RTK(sat[i],f,opt)]-lamj*x[IB_RTK(sat[j],f,opt)]; + if (H) + { + Hi[IB_RTK(sat[i],f,opt)]= lami; + Hi[IB_RTK(sat[j],f,opt)]=-lamj; + } + } + else + { + v[nv]-=x[IB_RTK(sat[i],f,opt)]-x[IB_RTK(sat[j],f,opt)]; + if (H) + { + Hi[IB_RTK(sat[i],f,opt)]= 1.0; + Hi[IB_RTK(sat[j],f,opt)]=-1.0; + } + } + } + /* glonass receiver h/w bias term */ + if (rtk->opt.glomodear==2&&sysi==SYS_GLO&&sysj==SYS_GLO&&ffopt,lami,lamj,f); + } + if (fssat[sat[j]-1].resc[f ]=v[nv]; + else rtk->ssat[sat[j]-1].resp[f-nf]=v[nv]; + + /* test innovation */ + if (opt->maxinno>0.0&&fabs(v[nv])>opt->maxinno) + { + if (fssat[sat[i]-1].rejc[f]++; + rtk->ssat[sat[j]-1].rejc[f]++; + } + errmsg(rtk,"outlier rejected (sat=%3d-%3d %s%d v=%.3f)\n", + sat[i],sat[j],fmode>PMODE_DGPS) + { + if (fssat[sat[i]-1].vsat[f]=rtk->ssat[sat[j]-1].vsat[f]=1; + } + else + { + rtk->ssat[sat[i]-1].vsat[f-nf]=rtk->ssat[sat[j]-1].vsat[f-nf]=1; + } + trace(4,"sat=%3d-%3d %s%d v=%13.3f R=%8.6f %8.6f\n",sat[i], + sat[j],fssat[j].resc[f]; + s/=nb[b]+1; + for (j=0;jssat[j].resc[f]!=0.0) rtk->ssat[j].resc[f]-=s; + } + } + else + { + for (j=0,s=0.0;jssat[j].resp[f-nf]; + s/=nb[b]+1; + for (j=0;jssat[j].resp[f-nf]!=0.0) + rtk->ssat[j].resp[f-nf]-=s; + } + } +#endif + b++; + } + /* end of system loop */ + + /* baseline length constraint for moving baseline */ + if (opt->mode==PMODE_MOVEB&&constbl(rtk,x,P,v,H,Ri,Rj,nv)) + { + vflg[nv++]=3<<4; + nb[b++]++; + } + if (H) {trace(5,"H=\n"); tracemat(5,H,rtk->nx,nv,7,4);} + + /* double-differenced measurement error covariance */ + ddcov(nb,b,Ri,Rj,nv,R); + + free(Ri); free(Rj); free(im); + free(tropu); free(tropr); free(dtdxu); free(dtdxr); + return nv; } -/* validate solution ---------------------------------------------------------*/ -int valsol(const double *azel, const int *vsat, int n, - const prcopt_t *opt, const double *v, int nv, int nx, - char *msg) +/* time-interpolation of residuals (for post-mission) ------------------------*/ +double intpres(gtime_t time, const obsd_t *obs, int n, const nav_t *nav, + rtk_t *rtk, double *y) { - double azels[MAXOBS*2], dop[4], vv; - int i, ns; + static obsd_t obsb[MAXOBS]; + static double yb[MAXOBS*NFREQ*2],rs[MAXOBS*6],dts[MAXOBS*2],var[MAXOBS]; + static double e[MAXOBS*3],azel[MAXOBS*2]; + static int nb=0,svh[MAXOBS*2]; + prcopt_t *opt=&rtk->opt; + double tt=timediff(time,obs[0].time),ttb,*p,*q; + int i,j,k,nf=NF_RTK(opt); - trace(3, "valsol : n=%d nv=%d\n", n, nv); + trace(3,"intpres : n=%d tt=%.1f\n",n,tt); - /* chi-square validation of residuals */ - vv = dot(v, v, nv); - if (nv>nx && vv>chisqr[nv-nx-1]) - { - sprintf(msg, "chi-square error nv=%d vv=%.1f cs=%.1f", nv, vv, chisqr[nv-nx-1]); - return 0; - } - /* large gdop check */ - for (i = ns = 0; ielmin, dop); - if (dop[0] <= 0.0 || dop[0]>opt->maxgdop) - { - sprintf(msg, "gdop error nv=%d gdop=%.1f", nv, dop[0]); - return 0; - } - return 1; + if (nb==0||fabs(tt)opt->maxtdiff*2.0||ttb==tt) return tt; + + satposs(time,obsb,nb,nav,opt->sateph,rs,dts,var,svh); + + if (!zdres(1,obsb,nb,rs,dts,svh,nav,rtk->rb,opt,1,yb,e,azel)) + { + return tt; + } + for (i=0;i=nb) continue; + for (k=0,p=y+i*nf*2,q=yb+j*nf*2;kfabs(tt)?ttb:tt; } -/* estimate receiver position ------------------------------------------------*/ -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) +/* single to double-difference transformation matrix (D') --------------------*/ +int ddmat(rtk_t *rtk, double *D) { - double x[NX] = {0}, dx[NX], Q[NX*NX], *v, *H, *var, sig; - int i, j, k, info, stat, nv, ns; + int i,j,k,m,f,nb=0,nx=rtk->nx,na=rtk->na,nf=NF_RTK(&rtk->opt),nofix; - trace(3, "estpos : n=%d\n", n); + trace(3,"ddmat :\n"); - v = mat(n+4, 1); H = mat(NX, n+4); var = mat(n+4, 1); - - for (i = 0; i<3; i++) x[i] = sol->rr[i]; - - for (i = 0; issat[i].fix[j]=0; } - /* weight by variance */ - for (j = 0; jtype = 0; - sol->time = timeadd(obs[0].time, -x[3] / SPEED_OF_LIGHT); - sol->dtr[0] = x[3] / SPEED_OF_LIGHT; /* receiver clock bias (s) */ - sol->dtr[1] = x[4] / SPEED_OF_LIGHT; /* glo-gps time offset (s) */ - sol->dtr[2] = x[5] / SPEED_OF_LIGHT; /* gal-gps time offset (s) */ - sol->dtr[3] = x[6] / SPEED_OF_LIGHT; /* bds-gps time offset (s) */ - for (j = 0; j<6; j++) sol->rr[j] = j<3 ? x[j] : 0.0; - for (j = 0; j<3; j++) sol->qr[j] = (float)Q[j+j*NX]; - sol->qr[3] = (float)Q[1]; /* cov xy */ - sol->qr[4] = (float)Q[2+NX]; /* cov yz */ - sol->qr[5] = (float)Q[2]; /* cov zx */ - sol->ns = (unsigned char)ns; - sol->age = sol->ratio = 0.0; + for (m=0;m<4;m++) + { /* m=0:gps/qzs/sbs,1:glo,2:gal,3:bds */ - /* validate solution */ - if ((stat = valsol(azel, vsat, n, opt, v, nv, NX, msg))) - { - sol->stat = opt->sateph == EPHOPT_SBAS ? SOLQ_SBAS : SOLQ_SINGLE; - } - free(v); free(H); free(var); + nofix=(m==1&&rtk->opt.glomodear==0)||(m==3&&rtk->opt.bdsmodear==0); - return stat; - } - } - if (i >= MAXITR) sprintf(msg, "iteration divergent i=%d", i); + for (f=0,k=na;frms) continue; - - /* save result */ - for (j = k = 0; jlam[obs[i].sat-1][0]; - - if (obs[i].D[0] == 0.0 || lam == 0.0 || !vsat[i] || norm(rs+3+i*6, 3) <= 0.0) - { - continue; - } - /* line-of-sight vector in ecef */ - cosel = cos(azel[1+i*2]); - a[0] = sin(azel[i*2])*cosel; - a[1] = cos(azel[i*2])*cosel; - a[2] = sin(azel[1+i*2]); - matmul("TN", 3, 1, 3, 1.0, E, a, 0.0, e); - - /* satellite velocity relative to receiver in ecef */ - for (j = 0; j<3; j++) vs[j] = rs[j+3+i*6]-x[j]; - - /* range rate with earth rotation correction */ - rate = dot(vs, e, 3)+DEFAULT_OMEGA_EARTH_DOT / SPEED_OF_LIGHT*(rs[4+i*6]*rr[0]+rs[1+i*6]*x[0]- - rs[3+i*6]*rr[1]-rs[ i*6]*x[1]); - - /* doppler residual */ - v[nv]=-lam*obs[i].D[0]-(rate+x[3]-SPEED_OF_LIGHT*dts[1+i*2]); - - /* design matrix */ - for (j = 0; j<4; j++) H[j+nv*4] = j<3 ? -e[j] : 1.0; - - nv++; - } - return nv; - } - - -/* estimate receiver velocity ------------------------------------------------*/ -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) -{ - double x[4] = {0}, dx[4], Q[16], *v, *H; - int i, j, nv; - - trace(3, "estvel : n=%d\n", n); - - v = mat(n, 1); H = mat(4, n); - - for (i = 0; irr, x, azel, vsat, v, H))<4) - { - break; - } - /* least square estimation */ - if (lsq(H, v, 4, nv, dx, Q)) break; - - for (j = 0; j<4; j++) x[j] += dx[j]; - - if (norm(dx, 4)<1E-6) - { - for (i = 0; i<3; i++) sol->rr[i+3] = x[i]; - break; - } - } - free(v); free(H); -} - - -int lexioncorr(gtime_t time, const nav_t *nav, const double *pos, - const double *azel, double *delay, double *var) -{ - const double re = 6378.137, hion = 350.0; - #if 0 - const double dl1=(141.0-129.0) / (45.5-34.7); - const double dl2=(129.0-126.7) / (34.7-26.0); - #endif - double tt, sinlat, coslat, sinaz, cosaz, cosel, rp, ap, sinap, cosap, latpp, lonpp; - double dlat, dlon, Enm, F; - int n, m; - - trace(4, "lexioncorr: time=%s pos=%.3f %.3f azel=%.3f %.3f\n", time_str(time, 3), - pos[0]*R2D, pos[1]*R2D, azel[0]*R2D, azel[1]*R2D); - - *delay = *var = 0.0; - - if (pos[2]<-100.0 || azel[1] <= 0.0) return 1; - - tt = timediff(time, nav->lexion.t0); - - /* check time span */ - if (fabs(tt)>nav->lexion.tspan) - { - trace(2, "lex iono age error: tt=%.0f tspan=%.0f\n", tt, nav->lexion.tspan); - return 0; - } - /* check user position range (ref [1] 4.1.5) */ - #if 0 - if (pos[0]> 45.5*D2R||pos[0]< 26.0*D2R|| - pos[1]>146.0*D2R|| - pos[1]<129.0*D2R+dl1*(pos[0]-34.7*D2R)|| - pos[1]<126.7*D2R+dl2*(pos[0]-26.0*D2R)) { - trace(2,"lex iono out of coverage pos=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D); - return 0; -} + for (i=k;ix[i]==0.0||!test_sys(rtk->ssat[i-k].sys,m)|| + !rtk->ssat[i-k].vsat[f]) + { +#else + if (rtk->x[i]==0.0||!test_sys(rtk->ssat[i-k].sys,m)|| + !rtk->ssat[i-k].vsat[f]||!rtk->ssat[i-k].half[f]) + { #endif -/* ionospheric pierce point position */ -sinlat = sin(pos[0]); -coslat = cos(pos[0]); -sinaz = sin(azel[0]); -cosaz = cos(azel[0]); -cosel = cos(azel[1]); -rp = re / (re+hion)*cosel; -ap = PI / 2.0-azel[1]-asin(rp); -sinap = sin(ap); -cosap = cos(ap); -latpp = asin(sinlat*cosap+coslat*sinap*cosaz); -lonpp = pos[1]+atan(sinap*sinaz / (cosap*coslat-sinap*cosaz*sinlat)); - -trace(4, "lexioncorr: pppos=%.3f %.3f\n", latpp*R2D, lonpp*R2D); - -/* inclination factor */ -F = 1.0 / sqrt(1.0-rp*rp); - -/* delta latitude/longitude (rad) */ -dlat = latpp-nav->lexion.pos0[0]; -dlon = lonpp-nav->lexion.pos0[1]; -trace(4, "lexioncorr: pos0=%.1f %.1f dlat=%.1f dlon=%.1f\n", -nav->lexion.pos0[0]*R2D, nav->lexion.pos0[1]*R2D, dlat*R2D, dlon*R2D); - -/* slant ionosphere delay (L1) */ -for (n = 0; n <= 2; n++) for (m = 0; m <= 1; m++) -{ - Enm = nav->lexion.coef[n][m]; - *delay += F*Enm*pow(dlat, n)*pow(dlon, m); - - trace(5, "lexioncorr: F=%8.3f Enm[%d][%d]=%8.3f delay=%8.3f\n", F, n, m, Enm, - F*Enm*pow(dlat, n)*pow(dlon, m)); -} -trace(4, "lexioncorr: time=%s delay=%.3f\n", time_str(time, 0), *delay); - -return 1; -} - - -/* single-point positioning ---------------------------------------------------- -* compute receiver position, velocity, clock bias by single-point positioning -* with pseudorange and doppler observables -* args : obsd_t *obs I observation data -* int n I number of observation data -* nav_t *nav I navigation data -* prcopt_t *opt I processing options -* sol_t *sol IO solution -* double *azel IO azimuth/elevation angle (rad) (NULL: no output) -* ssat_t *ssat IO satellite status (NULL: no output) -* char *msg O error message for error exit -* return : status(1:ok,0:error) -* notes : assuming sbas-gps, galileo-gps, qzss-gps, compass-gps time offset and -* receiver bias are negligible (only involving glonass-gps time offset -* and receiver bias) -*-----------------------------------------------------------------------------*/ -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) -{ - prcopt_t opt_ = *opt; - double *rs, *dts, *var, *azel_, *resp; - int i, stat, vsat[MAXOBS] = {0}, svh[MAXOBS]; - - sol->stat = SOLQ_NONE; - - if (n <= 0) {strcpy(msg, "no observation data"); return 0;} - - trace(3, "pntpos : tobs=%s n=%d\n", time_str(obs[0].time, 3), n); - - sol->time = obs[0].time; msg[0] = '\0'; - - rs=mat(6, n); dts = mat(2, n); var = mat(1, n); azel_ = zeros(2, n); resp = mat(1, n); - - if (opt_.mode!=PMODE_SINGLE) - { /* for precise positioning */ - #if 0 - opt_.sateph =EPHOPT_BRDC; - #endif - opt_.ionoopt = IONOOPT_BRDC; - opt_.tropopt = TROPOPT_SAAS; - } - /* satellite positons, velocities and clocks */ - satposs(sol->time, obs, n, nav, opt_.sateph, rs, dts, var, svh); - - /* estimate receiver position with pseudorange */ - stat = estpos(obs, n, rs, dts, var, svh, nav, &opt_, sol, azel_, vsat, resp, msg); - - /* raim fde */ - if (!stat && n >= 6 && opt->posopt[4]) - { - stat = raim_fde(obs, n, rs, dts, var, svh, nav, &opt_, sol, azel_, vsat, resp, msg); - } - /* estimate receiver velocity with doppler */ - if (stat) estvel(obs, n, rs, dts, nav, &opt_, sol, azel_, vsat); - - if (azel) - { - for (i = 0; issat[i-k].lock[f]>0&&!(rtk->ssat[i-k].slip[f]&2)&& + rtk->ssat[i-k].azel[1]>=rtk->opt.elmaskar&&!nofix) + { + rtk->ssat[i-k].fix[f]=2; /* fix */ + break; + } + else rtk->ssat[i-k].fix[f]=1; + } + for (j=k;jx[j]==0.0||!test_sys(rtk->ssat[j-k].sys,m)|| + !rtk->ssat[j-k].vsat[f]) + { + continue; + } + if (rtk->ssat[j-k].lock[f]>0&&!(rtk->ssat[j-k].slip[f]&2)&& + rtk->ssat[i-k].vsat[f]&& + rtk->ssat[j-k].azel[1]>=rtk->opt.elmaskar&&!nofix) + { + D[i+(na+nb)*nx]= 1.0; + D[j+(na+nb)*nx]=-1.0; + nb++; + rtk->ssat[j-k].fix[f]=2; /* fix */ + } + else rtk->ssat[j-k].fix[f]=1; + } + } + } + trace(5,"D=\n"); tracemat(5,D,nx,na+nb,2,0); + return nb; } + + + /* restore single-differenced ambiguity --------------------------------------*/ + void restamb(rtk_t *rtk, const double *bias, int nb, double *xa) + { + int i,n,m,f,index[MAXSAT],nv=0,nf=NF_RTK(&rtk->opt); + + trace(3,"restamb :\n"); + + for (i=0;inx;i++) xa[i]=rtk->x [i]; + for (i=0;ina;i++) xa[i]=rtk->xa[i]; + + for (m=0;m<4;m++) for (f=0;fssat[i].sys,m)||rtk->ssat[i].fix[f]!=2) + { + continue; + } + index[n++]=IB_RTK(i+1,f,&rtk->opt); + } + if (n<2) continue; + + xa[index[0]]=rtk->x[index[0]]; + + for (i=1;inx-rtk->na,nv=0,nf=NF_RTK(&rtk->opt); + + trace(3,"holdamb :\n"); + + v=mat(nb,1); H=zeros(nb,rtk->nx); + + for (m=0;m<4;m++) for (f=0;fssat[i].sys,m)||rtk->ssat[i].fix[f]!=2|| + rtk->ssat[i].azel[1]opt.elmaskhold) + { + continue; + } + index[n++]=IB_RTK(i+1,f,&rtk->opt); + rtk->ssat[i].fix[f]=3; /* hold */ + } + /* constraint to fixed ambiguity */ + for (i=1;ix[index[0]]-rtk->x[index[i]]); + + H[index[0]+nv*rtk->nx]= 1.0; + H[index[i]+nv*rtk->nx]=-1.0; + nv++; + } + } + if (nv>0) + { + R=zeros(nv,nv); + for (i=0;ix,rtk->P,H,v,R,rtk->nx,nv))) + { + errmsg(rtk,"filter error (info=%d)\n",info); + } + free(R); + } + free(v); free(H); + } + + + /* resolve integer ambiguity by LAMBDA ---------------------------------------*/ + int resamb_LAMBDA(rtk_t *rtk, double *bias, double *xa) + { + prcopt_t *opt=&rtk->opt; + int i,j,ny,nb,info,nx=rtk->nx,na=rtk->na; + double *D,*DP,*y,*Qy,*b,*db,*Qb,*Qab,*QQ,s[2]; + + trace(3,"resamb_LAMBDA : nx=%d\n",nx); + + rtk->sol.ratio=0.0; + + if (rtk->opt.mode<=PMODE_DGPS||rtk->opt.modear==ARMODE_OFF|| + rtk->opt.thresar[0]<1.0) + { + return 0; + } + /* single to double-difference transformation matrix (D') */ + D=zeros(nx,nx); + if ((nb=ddmat(rtk,D))<=0) + { + errmsg(rtk,"no valid double-difference\n"); + free(D); + return 0; + } + ny=na+nb; y=mat(ny,1); Qy=mat(ny,ny); DP=mat(ny,nx); + b=mat(nb,2); db=mat(nb,1); Qb=mat(nb,nb); Qab=mat(na,nb); QQ=mat(na,nb); + + /* transform single to double-differenced phase-bias (y=D'*x, Qy=D'*P*D) */ + matmul("TN",ny, 1,nx,1.0,D ,rtk->x,0.0,y ); + matmul("TN",ny,nx,nx,1.0,D ,rtk->P,0.0,DP); + matmul("NN",ny,ny,nx,1.0,DP,D ,0.0,Qy); + + /* phase-bias covariance (Qb) and real-parameters to bias covariance (Qab) */ + for (i=0;ixa); + + /* covariance of fixed solution (Qa=Qa-Qab*Qb^-1*Qab') */ + matmul("NN",na,nb,nb, 1.0,Qab,Qb ,0.0,QQ); + matmul("NT",na,na,nb,-1.0,QQ ,Qab,1.0,rtk->Pa); + + trace(3,"resamb : validation ok (nb=%d ratio=%.2f s=%.2f/%.2f)\n", + nb,s[0]==0.0?0.0:s[1]/s[0],s[0],s[1]); + + /* restore single-differenced ambiguity */ + restamb(rtk,bias,nb,xa); + } + else nb=0; + } + else + { /* validation failed */ + errmsg(rtk,"ambiguity validation failed (nb=%d ratio=%.2f s=%.2f/%.2f)\n", + nb,s[1]/s[0],s[0],s[1]); + nb=0; + } + } + else + { + errmsg(rtk,"lambda error (info=%d)\n",info); + } + free(D); free(y); free(Qy); free(DP); + free(b); free(db); free(Qb); free(Qab); free(QQ); + + return nb; /* number of ambiguities */ + } + + + /* validation of solution ----------------------------------------------------*/ + int valpos(rtk_t *rtk, const double *v, const double *R, const int *vflg, + int nv, double thres) + { +#if 0 + prcopt_t *opt=&rtk->opt; + double vv=0.0; +#endif + double fact=thres*thres; + int i,stat=1,sat1,sat2,type,freq; + char stype; + + trace(3,"valpos : nv=%d thres=%.1f\n",nv,thres); + + /* post-fit residual test */ + for (i=0;i>16)&0xFF; + sat2=(vflg[i]>> 8)&0xFF; + type=(vflg[i]>> 4)&0xF; + freq=vflg[i]&0xF; + stype=type==0?'L':(type==1?'L':'C'); + errmsg(rtk,"large residual (sat=%2d-%2d %s%d v=%6.3f sig=%.3f)\n", + sat1,sat2,stype,freq+1,v[i],std::sqrt(R[i+i*nv])); + } +#if 0 /* omitted v.2.4.0 */ + if (stat&&nv>NP(opt)) + { + + /* chi-square validation */ + for (i=0;ichisqr[nv-NP(opt)-1]) + { + errmsg(rtk,"residuals validation failed (nv=%d np=%d vv=%.2f cs=%.2f)\n", + nv,NP(opt),vv,chisqr[nv-NP(opt)-1]); + stat=0; + } + else + { + trace(3,"valpos : validation ok (%s nv=%d np=%d vv=%.2f cs=%.2f)\n", + rtk->tstr,nv,NP(opt),vv,chisqr[nv-NP(opt)-1]); + } + } +#endif + return stat; + } + + + /* relative positioning ------------------------------------------------------*/ + int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr, + const nav_t *nav) + { + prcopt_t *opt=&rtk->opt; + gtime_t time=obs[0].time; + double *rs,*dts,*var,*y,*e,*azel,*v,*H,*R,*xp,*Pp,*xa,*bias,dt; + int i,j,f,n=nu+nr,ns,ny,nv,sat[MAXSAT],iu[MAXSAT],ir[MAXSAT],niter; + int info,vflg[MAXOBS*NFREQ*2+1],svh[MAXOBS*2]; + int stat=rtk->opt.mode<=PMODE_DGPS?SOLQ_DGPS:SOLQ_FLOAT; + int nf=opt->ionoopt==IONOOPT_IFLC?1:opt->nf; + + trace(3,"relpos : nx=%d nu=%d nr=%d\n",rtk->nx,nu,nr); + + dt=timediff(time,obs[nu].time); + + rs=mat(6,n); dts=mat(2,n); var=mat(1,n); y=mat(nf*2,n); e=mat(3,n); + azel=zeros(2,n); + + for (i=0;issat[i].sys=satsys(i+1,NULL); + for (j=0;jssat[i].vsat[j]=rtk->ssat[i].snr[j]=0; + } + /* satellite positions/clocks */ + satposs(time,obs,n,nav,opt->sateph,rs,dts,var,svh); + + /* undifferenced residuals for base station */ + if (!zdres(1,obs+nu,nr,rs+nu*6,dts+nu*2,svh+nu,nav,rtk->rb,opt,1, + y+nu*nf*2,e+nu*3,azel+nu*2)) + { + errmsg(rtk,"initial base station position error\n"); + + free(rs); free(dts); free(var); free(y); free(e); free(azel); + return 0; + } + /* time-interpolation of residuals (for post-processing) */ + if (opt->intpref) + { + dt=intpres(time,obs+nu,nr,nav,rtk,y+nu*nf*2); + } + /* select common satellites between rover and base-station */ + if ((ns=selsat(obs,azel,nu,nr,opt,sat,iu,ir))<=0) + { + errmsg(rtk,"no common satellite\n"); + + free(rs); free(dts); free(var); free(y); free(e); free(azel); + return 0; + } + /* temporal update of states */ + udstate(rtk,obs,sat,iu,ir,ns,nav); + + trace(4,"x(0)="); tracemat(4,rtk->x,1,NR_RTK(opt),13,4); + + xp=mat(rtk->nx,1); Pp=zeros(rtk->nx,rtk->nx); xa=mat(rtk->nx,1); + matcpy(xp,rtk->x,rtk->nx,1); + + ny=ns*nf*2+2; + v=mat(ny,1); H=zeros(rtk->nx,ny); R=mat(ny,ny); bias=mat(rtk->nx,1); + + /* add 2 iterations for baseline-constraint moving-base */ + niter=opt->niter+(opt->mode==PMODE_MOVEB&&opt->baseline[0]>0.0?2:0); + + for (i=0;iP,rtk->nx,rtk->nx); + if ((info=filter(xp,Pp,H,v,R,rtk->nx,nv))) + { + errmsg(rtk,"filter error (info=%d)\n",info); + stat=SOLQ_NONE; + break; + } + trace(4,"x(%d)=",i+1); tracemat(4,xp,1,NR_RTK(opt),13,4); + } + if (stat!=SOLQ_NONE&&zdres(0,obs,nu,rs,dts,svh,nav,xp,opt,0,y,e,azel)) + { + + /* post-fit residuals for float solution */ + nv=ddres(rtk,nav,dt,xp,Pp,sat,y,e,azel,iu,ir,ns,v,NULL,R,vflg); + + /* validation of float solution */ + if (valpos(rtk,v,R,vflg,nv,4.0)) + { + + /* update state and covariance matrix */ + matcpy(rtk->x,xp,rtk->nx,1); + matcpy(rtk->P,Pp,rtk->nx,rtk->nx); + + /* update ambiguity control struct */ + rtk->sol.ns=0; + for (i=0;issat[sat[i]-1].vsat[f]) continue; + rtk->ssat[sat[i]-1].lock[f]++; + rtk->ssat[sat[i]-1].outc[f]=0; + if (f==0) rtk->sol.ns++; /* valid satellite count by L1 */ + } + /* lack of valid satellites */ + if (rtk->sol.ns<4) stat=SOLQ_NONE; + } + else stat=SOLQ_NONE; + } + /* resolve integer ambiguity by WL-NL */ + if (stat!=SOLQ_NONE&&rtk->opt.modear==ARMODE_WLNL) + { + if (resamb_WLNL(rtk,obs,sat,iu,ir,ns,nav,azel)) + { + stat=SOLQ_FIX; + } + } + /* resolve integer ambiguity by TCAR */ + else if (stat!=SOLQ_NONE&&rtk->opt.modear==ARMODE_TCAR) + { + if (resamb_TCAR(rtk,obs,sat,iu,ir,ns,nav,azel)) + { + stat=SOLQ_FIX; + } + } + /* resolve integer ambiguity by LAMBDA */ + else if (stat!=SOLQ_NONE&&resamb_LAMBDA(rtk,bias,xa)>1) + { + + if (zdres(0,obs,nu,rs,dts,svh,nav,xa,opt,0,y,e,azel)) + { + + /* post-fit reisiduals for fixed solution */ + nv=ddres(rtk,nav,dt,xa,NULL,sat,y,e,azel,iu,ir,ns,v,NULL,R,vflg); + + /* validation of fixed solution */ + if (valpos(rtk,v,R,vflg,nv,4.0)) + { + + /* hold integer ambiguity */ + if (++rtk->nfix>=rtk->opt.minfix&& + rtk->opt.modear==ARMODE_FIXHOLD) + { + holdamb(rtk,xa); + } + stat=SOLQ_FIX; + } + } + } + /* save solution status */ + if (stat==SOLQ_FIX) + { + for (i=0;i<3;i++) + { + rtk->sol.rr[i]=rtk->xa[i]; + rtk->sol.qr[i]=(float)rtk->Pa[i+i*rtk->na]; + } + rtk->sol.qr[3]=(float)rtk->Pa[1]; + rtk->sol.qr[4]=(float)rtk->Pa[1+2*rtk->na]; + rtk->sol.qr[5]=(float)rtk->Pa[2]; + } + else + { + for (i=0;i<3;i++) + { + rtk->sol.rr[i]=rtk->x[i]; + rtk->sol.qr[i]=(float)rtk->P[i+i*rtk->nx]; + } + rtk->sol.qr[3]=(float)rtk->P[1]; + rtk->sol.qr[4]=(float)rtk->P[1+2*rtk->nx]; + rtk->sol.qr[5]=(float)rtk->P[2]; + rtk->nfix=0; + } + for (i=0;issat[obs[i].sat-1].pt[obs[i].rcv-1][j]=obs[i].time; + rtk->ssat[obs[i].sat-1].ph[obs[i].rcv-1][j]=obs[i].L[j]; + } + for (i=0;issat[sat[i]-1].snr[j]=obs[iu[i]].SNR[j]; + } + for (i=0;issat[i].fix[j]==2&&stat!=SOLQ_FIX) rtk->ssat[i].fix[j]=1; + if (rtk->ssat[i].slip[j]&1) rtk->ssat[i].slipc[j]++; + } + free(rs); free(dts); free(var); free(y); free(e); free(azel); + free(xp); free(Pp); free(xa); free(v); free(H); free(R); free(bias); + + if (stat!=SOLQ_NONE) rtk->sol.stat=stat; + + return stat!=SOLQ_NONE; + } + + + /* initialize rtk control ------------------------------------------------------ + * initialize rtk control struct + * args : rtk_t *rtk IO rtk control/result struct + * prcopt_t *opt I positioning options (see rtklib.h) + * return : none + *-----------------------------------------------------------------------------*/ + void rtkinit(rtk_t *rtk, const prcopt_t *opt) + { + sol_t sol0={{0}}; + ambc_t ambc0={{{0}}}; + ssat_t ssat0={0}; + int i; + + trace(3,"rtkinit :\n"); + + rtk->sol=sol0; + for (i=0;i<6;i++) rtk->rb[i]=0.0; + rtk->nx=opt->mode<=PMODE_FIXED?NX_RTK(opt):pppnx(opt); + rtk->na=opt->mode<=PMODE_FIXED?NR_RTK(opt):pppnx(opt); + rtk->tt=0.0; + rtk->x=zeros(rtk->nx,1); + rtk->P=zeros(rtk->nx,rtk->nx); + rtk->xa=zeros(rtk->na,1); + rtk->Pa=zeros(rtk->na,rtk->na); + rtk->nfix=rtk->neb=0; + for (i=0;iambc[i]=ambc0; + rtk->ssat[i]=ssat0; + } + for (i=0;ierrbuf[i]=0; + rtk->opt=*opt; + } + + + /* free rtk control ------------------------------------------------------------ + * free memory for rtk control struct + * args : rtk_t *rtk IO rtk control/result struct + * return : none + *-----------------------------------------------------------------------------*/ + void rtkfree(rtk_t *rtk) + { + trace(3,"rtkfree :\n"); + + rtk->nx=rtk->na=0; + free(rtk->x ); rtk->x =NULL; + free(rtk->P ); rtk->P =NULL; + free(rtk->xa); rtk->xa=NULL; + free(rtk->Pa); rtk->Pa=NULL; + } + + + /* precise positioning --------------------------------------------------------- + * input observation data and navigation message, compute rover position by + * precise positioning + * args : rtk_t *rtk IO rtk control/result struct + * rtk->sol IO solution + * .time O solution time + * .rr[] IO rover position/velocity + * (I:fixed mode,O:single mode) + * .dtr[0] O receiver clock bias (s) + * .dtr[1] O receiver glonass-gps time offset (s) + * .Qr[] O rover position covarinace + * .stat O solution status (SOLQ_???) + * .ns O number of valid satellites + * .age O age of differential (s) + * .ratio O ratio factor for ambiguity validation + * rtk->rb[] IO base station position/velocity + * (I:relative mode,O:moving-base mode) + * rtk->nx I number of all states + * rtk->na I number of integer states + * rtk->ns O number of valid satellite + * rtk->tt O time difference between current and previous (s) + * rtk->x[] IO float states pre-filter and post-filter + * rtk->P[] IO float covariance pre-filter and post-filter + * rtk->xa[] O fixed states after AR + * rtk->Pa[] O fixed covariance after AR + * rtk->ssat[s] IO sat(s+1) status + * .sys O system (SYS_???) + * .az [r] O azimuth angle (rad) (r=0:rover,1:base) + * .el [r] O elevation angle (rad) (r=0:rover,1:base) + * .vs [r] O data valid single (r=0:rover,1:base) + * .resp [f] O freq(f+1) pseudorange residual (m) + * .resc [f] O freq(f+1) carrier-phase residual (m) + * .vsat [f] O freq(f+1) data vaild (0:invalid,1:valid) + * .fix [f] O freq(f+1) ambiguity flag + * (0:nodata,1:float,2:fix,3:hold) + * .slip [f] O freq(f+1) slip flag + * (bit8-7:rcv1 LLI, bit6-5:rcv2 LLI, + * bit2:parity unknown, bit1:slip) + * .lock [f] IO freq(f+1) carrier lock count + * .outc [f] IO freq(f+1) carrier outage count + * .slipc[f] IO freq(f+1) cycle slip count + * .rejc [f] IO freq(f+1) data reject count + * .gf IO geometry-free phase (L1-L2) (m) + * .gf2 IO geometry-free phase (L1-L5) (m) + * rtk->nfix IO number of continuous fixes of ambiguity + * rtk->neb IO bytes of error message buffer + * rtk->errbuf IO error message buffer + * rtk->tstr O time string for debug + * rtk->opt I processing options + * obsd_t *obs I observation data for an epoch + * obs[i].rcv=1:rover,2:reference + * sorted by receiver and satellte + * int n I number of observation data + * nav_t *nav I navigation messages + * return : status (0:no solution,1:valid solution) + * notes : before calling function, base station position rtk->sol.rb[] should + * be properly set for relative mode except for moving-baseline + *-----------------------------------------------------------------------------*/ + int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) + { + prcopt_t *opt=&rtk->opt; + sol_t solb={{0}}; + gtime_t time; + int i,nu,nr; + char msg[128]=""; + + trace(3,"rtkpos : time=%s n=%d\n",time_str(obs[0].time,3),n); + trace(4,"obs=\n"); traceobs(4,obs,n); + /*trace(5,"nav=\n"); tracenav(5,nav);*/ + + /* set base staion position */ + if (opt->refpos<=POSOPT_RINEX&&opt->mode!=PMODE_SINGLE&& + opt->mode!=PMODE_MOVEB) + { + for (i=0;i<6;i++) rtk->rb[i]=i<3?opt->rb[i]:0.0; + } + /* count rover/base station observations */ + for (nu=0;nu sol.time; /* previous epoch */ + + /* rover position by single point positioning */ + if (!pntpos(obs,nu,nav,&rtk->opt,&rtk->sol,NULL,rtk->ssat,msg)) + { + errmsg(rtk,"point pos error (%s)\n",msg); + + if (!rtk->opt.dynamics) + { + outsolstat(rtk); + return 0; + } + } + if (time.time!=0) rtk->tt=timediff(rtk->sol.time,time); + + /* single point positioning */ + if (opt->mode==PMODE_SINGLE) + { + outsolstat(rtk); + return 1; + } + /* suppress output of single solution */ + if (!opt->outsingle) + { + rtk->sol.stat=SOLQ_NONE; + } + /* precise point positioning */ + if (opt->mode>=PMODE_PPP_KINEMA) + { + pppos(rtk,obs,nu,nav); + outsolstat(rtk); + return 1; + } + /* check number of data of base station and age of differential */ + if (nr==0) + { + errmsg(rtk,"no base station observation data for rtk\n"); + outsolstat(rtk); + return 1; + } + if (opt->mode==PMODE_MOVEB) + { /* moving baseline */ + + /* estimate position/velocity of base station */ + if (!pntpos(obs+nu,nr,nav,&rtk->opt,&solb,NULL,NULL,msg)) + { + errmsg(rtk,"base station position error (%s)\n",msg); + return 0; + } + rtk->sol.age=(float)timediff(rtk->sol.time,solb.time); + + if (fabs(rtk->sol.age)>TTOL_MOVEB) + { + errmsg(rtk,"time sync error for moving-base (age=%.1f)\n",rtk->sol.age); + return 0; + } + for (i=0;i<6;i++) rtk->rb[i]=solb.rr[i]; + + /* time-synchronized position of base station */ + for (i=0;i<3;i++) rtk->rb[i]+=rtk->rb[i+3]*rtk->sol.age; + } + else + { + rtk->sol.age=(float)timediff(obs[0].time,obs[nu].time); + + if (fabs(rtk->sol.age)>opt->maxtdiff) + { + errmsg(rtk,"age of differential error (age=%.1f)\n",rtk->sol.age); + outsolstat(rtk); + return 1; + } + } + /* relative potitioning */ + relpos(rtk,obs,nu,nr,nav); + outsolstat(rtk); + + return 1; } - free(rs); free(dts); free(var); free(azel_); free(resp); - return stat; -} diff --git a/src/algorithms/libs/rtklib/rtklib_rtkpos.h b/src/algorithms/libs/rtklib/rtklib_rtkpos.h index 9d7a66ab4..434a22375 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkpos.h +++ b/src/algorithms/libs/rtklib/rtklib_rtkpos.h @@ -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)->tropopttropoptglomodear!=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 diff --git a/src/algorithms/libs/rtklib/rtklib_sbas.cc b/src/algorithms/libs/rtklib/rtklib_sbas.cc index 05f41f743..b6c0afe1e 100644 --- a/src/algorithms/libs/rtklib/rtklib_sbas.cc +++ b/src/algorithms/libs/rtklib/rtklib_sbas.cc @@ -60,7 +60,7 @@ *----------------------------------------------------------------------------*/ #include "rtklib_sbas.h" - +#include "rtklib_rtkcmn.h" /* extract field from line ---------------------------------------------------*/ char *getfield(char *p, int pos) diff --git a/src/algorithms/libs/rtklib/rtklib_sbas.h b/src/algorithms/libs/rtklib/rtklib_sbas.h index 13933236b..62ea1dd31 100644 --- a/src/algorithms/libs/rtklib/rtklib_sbas.h +++ b/src/algorithms/libs/rtklib/rtklib_sbas.h @@ -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 diff --git a/src/algorithms/libs/rtklib/rtklib_tides.cc b/src/algorithms/libs/rtklib/rtklib_tides.cc new file mode 100644 index 000000000..21e0652cd --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_tides.cc @@ -0,0 +1,321 @@ +/*! + * \file rtklib_tides.cc + * \brief Tidal displacement corrections + * \authors
      + *
    • 2007-2008, T. Takasu + *
    • 2017, Javier Arribas + *
    • 2017, Carles Fernandez + *
    + * + * 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]); +} diff --git a/src/algorithms/libs/rtklib/rtklib_tides.h b/src/algorithms/libs/rtklib/rtklib_tides.h new file mode 100644 index 000000000..44124604a --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_tides.h @@ -0,0 +1,92 @@ +/*! + * \file rtklib_tides.h + * \brief Tidal displacement corrections + * \authors
      + *
    • 2015, T. Takasu + *
    • 2017, Javier Arribas + *
    • 2017, Carles Fernandez + *
    + * + * 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