From 60501b53faa8ec2a4b46c8f408d6b45f3bb87eb2 Mon Sep 17 00:00:00 2001 From: Vladslav P Date: Sat, 25 Jul 2026 19:55:12 +0300 Subject: [PATCH 1/7] GPS CNAV: fix satellite position computation Implement satellite positions computation following the procedure, described on page 166 of IS-GPS-200 (08/01/2022) Implement L2C/L5I group delay compensation following the procedure, described on page 175 of IS-GPS-200 (08/01/2022) Signed-off-by: Vladslav P --- .../libs/rtklib/rtklib_ephemeris.cc | 10 ++++-- src/algorithms/libs/rtklib/rtklib_pntpos.cc | 17 ++++++--- src/algorithms/libs/rtklib/rtklib_pntpos.h | 2 +- src/core/system_parameters/gnss_ephemeris.cc | 9 +++-- src/core/system_parameters/gnss_ephemeris.h | 36 ++++++++++--------- .../system_parameters/gps_cnav_ephemeris.h | 2 -- 6 files changed, 46 insertions(+), 30 deletions(-) diff --git a/src/algorithms/libs/rtklib/rtklib_ephemeris.cc b/src/algorithms/libs/rtklib/rtklib_ephemeris.cc index 30941edd6..8ba8cc7e7 100644 --- a/src/algorithms/libs/rtklib/rtklib_ephemeris.cc +++ b/src/algorithms/libs/rtklib/rtklib_ephemeris.cc @@ -350,6 +350,9 @@ void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts, double *var) { double tk; + double Ak; + double na; + double delta_na; double M; double E; double Ek; @@ -402,7 +405,10 @@ void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts, omge = GNSS_OMEGA_EARTH_DOT; break; } - M = eph->M0 + (sqrt(mu / (eph->A * eph->A * eph->A)) + eph->deln) * tk; + Ak = eph->A + eph->Adot * tk; + delta_na = eph->deln + 0.5 * eph->ndot * tk; + na = sqrt(mu / (eph->A * eph->A * eph->A)) + delta_na; + M = eph->M0 + na * tk; for (n = 0, E = M, Ek = 0.0; fabs(E - Ek) > RTOL_KEPLER && n < MAX_ITER_KEPLER; n++) { @@ -420,7 +426,7 @@ void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts, trace(4, "kepler: sat=%2d e=%8.5f n=%2d del=%10.3e\n", eph->sat, eph->e, n, E - Ek); u = atan2(sqrt(1.0 - eph->e * eph->e) * sinE, cosE - eph->e) + eph->omg; - r = eph->A * (1.0 - eph->e * cosE); + r = Ak * (1.0 - eph->e * cosE); i = eph->i0 + eph->idot * tk; sin2u = sin(2.0 * u); cos2u = cos(2.0 * u); diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index 8761e5e92..099a39ab5 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -163,7 +163,7 @@ double getiscl5q(int sat, const nav_t *nav) /* 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) + int iter, const prcopt_t *opt, double *var, int *fidx) { const double *lam = nav->lam[obs->sat - 1]; double PC = 0.0; @@ -296,6 +296,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, { P1 += P1_C1; /* C1->P1 */ PC = P1 - P1_P2; + *fidx = i; } else if (obs->code[i] == CODE_NONE && obs->code[j] != CODE_NONE) { @@ -305,17 +306,20 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, // PC = P2 - gamma_ * P1_P2 / (1.0 - gamma_); if (obs->code[j] == CODE_L2S) // L2 single freq. { - PC = P2 + P1_P2 - ISCl2; + PC = P2 - P1_P2 + ISCl2; + *fidx = j; } else if (obs->code[j] == CODE_L5X) // L5 single freq. { - PC = P2 + P1_P2 - ISCl5i; + PC = P2 - P1_P2 + ISCl5i; + *fidx = j; } } if (sys == SYS_BDS) { P2 += P2_C2; /* C2->P2 */ PC = P2; // no tgd corrections for B3I + *fidx = j; } else if (sys == SYS_GAL) { @@ -323,11 +327,13 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, // Galileo OS SIS ICD, Eq. 19: E5a/E5b uses // (f_E1/f_E5)^2 times its corresponding BGD. PC = uses_galileo_bgd ? P2 - gamma_ * P1_P2 : P2 - gamma_ * P1_P2 / (1.0 - gamma_); + *fidx = j; } else if (sys == SYS_GLO) { P2 += P2_C2; /* C2->P2 */ PC = P2 - gamma_ * P1_P2 / (1.0 - gamma_); + *fidx = j; } } /* dual-frequency */ @@ -477,6 +483,7 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs, int nv = 0; int sys; int mask[4] = {0}; + int fidx = 0; trace(3, "resprng : n=%d\n", n); @@ -519,7 +526,7 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs, continue; } /* psudorange with code bias correction */ - if ((P = prange(obs + i, nav, azel + i * 2, iter, opt, &vmeas)) == 0.0) + if ((P = prange(obs + i, nav, azel + i * 2, iter, opt, &vmeas, &fidx)) == 0.0) { trace(4, "prange error\n"); continue; @@ -541,7 +548,7 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs, } /* GPS-L1 -> L1/B1 */ - if ((lam_L1 = nav->lam[obs[i].sat - 1][0]) > 0.0) + if ((lam_L1 = nav->lam[obs[i].sat - 1][fidx]) > 0.0) { dion *= std::pow(lam_L1 / LAM_CARR[0], 2.0); } diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.h b/src/algorithms/libs/rtklib/rtklib_pntpos.h index 9afd45974..6238ee222 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.h +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.h @@ -61,7 +61,7 @@ double getiscl5q(int sat, const nav_t *nav); /* 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); + int iter, const prcopt_t *opt, double *var, int *fidx); /* ionospheric correction ------------------------------------------------------ * compute ionospheric correction diff --git a/src/core/system_parameters/gnss_ephemeris.cc b/src/core/system_parameters/gnss_ephemeris.cc index b1c8ec2fe..a518e7d17 100644 --- a/src/core/system_parameters/gnss_ephemeris.cc +++ b/src/core/system_parameters/gnss_ephemeris.cc @@ -190,8 +190,11 @@ void Gnss_Ephemeris::satellitePosVelComputation(double transmitTime, std::array< // Time from ephemeris reference epoch double tk = check_t(transmitTime - static_cast(this->toe)); + // Semi-major axis correction (CNAV) + const double Ak = a + this->Adot * tk; + // Corrected mean motion - const double n = n0 + this->delta_n; + const double n = n0 + this->delta_n + 0.5 * this->delta_ndot * tk; // Mean anomaly const double M = this->M_0 + n * tk; @@ -240,8 +243,8 @@ void Gnss_Ephemeris::satellitePosVelComputation(double transmitTime, std::array< const double ukdot = pkdot * (1.0 + 2.0 * (this->Cus * c2pk - this->Cuc * s2pk)); // Correct radius - const double r = a * OneMinusecosE + this->Crc * c2pk + this->Crs * s2pk; - const double rkdot = a * this->ecc * sek * ekdot + 2.0 * pkdot * (this->Crs * c2pk - this->Crc * s2pk); + const double r = Ak * OneMinusecosE + this->Crc * c2pk + this->Crs * s2pk; + const double rkdot = this->Adot * (1. - this->ecc * cek) + a * this->ecc * sek * ekdot + 2.0 * pkdot * (this->Crs * c2pk - this->Crc * s2pk); // Correct inclination const double i = this->i_0 + this->idot * tk + this->Cic * c2pk + this->Cis * s2pk; diff --git a/src/core/system_parameters/gnss_ephemeris.h b/src/core/system_parameters/gnss_ephemeris.h index 8616ea4c8..795d050e4 100644 --- a/src/core/system_parameters/gnss_ephemeris.h +++ b/src/core/system_parameters/gnss_ephemeris.h @@ -67,23 +67,25 @@ public: void satellitePosition(double transmitTime); //!< Computes the ECEF SV coordinates and ECEF velocity - uint32_t PRN{}; //!< SV ID - double M_0{}; //!< Mean anomaly at reference time [rad] - double delta_n{}; //!< Mean motion difference from computed value [rad/sec] - double ecc{}; //!< Eccentricity - double sqrtA{}; //!< Square root of the semi-major axis [meters^1/2] - double OMEGA_0{}; //!< Longitude of ascending node of orbital plane at weekly epoch [rad] - double i_0{}; //!< Inclination angle at reference time [rad] - double omega{}; //!< Argument of perigee [rad] - double OMEGAdot{}; //!< Rate of right ascension [rad/sec] - double idot{}; //!< Rate of inclination angle [rad/sec] - double Cuc{}; //!< Amplitude of the cosine harmonic correction term to the argument of latitude [rad] - double Cus{}; //!< Amplitude of the sine harmonic correction term to the argument of latitude [rad] - double Crc{}; //!< Amplitude of the cosine harmonic correction term to the orbit radius [meters] - double Crs{}; //!< Amplitude of the sine harmonic correction term to the orbit radius [meters] - double Cic{}; //!< Amplitude of the cosine harmonic correction term to the angle of inclination [rad] - double Cis{}; //!< Amplitude of the sine harmonic correction term to the angle of inclination [rad] - int32_t toe{}; //!< Ephemeris reference time [s] + uint32_t PRN{}; //!< SV ID + double M_0{}; //!< Mean anomaly at reference time [rad] + double Adot{}; //!< Change rate in semi-major axis (CNAV) + double delta_ndot{}; //!< Rate of mean motion difference from computed value (CNAV) + double delta_n{}; //!< Mean motion difference from computed value [rad/sec] + double ecc{}; //!< Eccentricity + double sqrtA{}; //!< Square root of the semi-major axis [meters^1/2] + double OMEGA_0{}; //!< Longitude of ascending node of orbital plane at weekly epoch [rad] + double i_0{}; //!< Inclination angle at reference time [rad] + double omega{}; //!< Argument of perigee [rad] + double OMEGAdot{}; //!< Rate of right ascension [rad/sec] + double idot{}; //!< Rate of inclination angle [rad/sec] + double Cuc{}; //!< Amplitude of the cosine harmonic correction term to the argument of latitude [rad] + double Cus{}; //!< Amplitude of the sine harmonic correction term to the argument of latitude [rad] + double Crc{}; //!< Amplitude of the cosine harmonic correction term to the orbit radius [meters] + double Crs{}; //!< Amplitude of the sine harmonic correction term to the orbit radius [meters] + double Cic{}; //!< Amplitude of the cosine harmonic correction term to the angle of inclination [rad] + double Cis{}; //!< Amplitude of the sine harmonic correction term to the angle of inclination [rad] + int32_t toe{}; //!< Ephemeris reference time [s] // Clock correction parameters int32_t toc{}; //!< Clock correction data reference Time of Week [sec] diff --git a/src/core/system_parameters/gps_cnav_ephemeris.h b/src/core/system_parameters/gps_cnav_ephemeris.h index 3bf156cca..1d5131726 100644 --- a/src/core/system_parameters/gps_cnav_ephemeris.h +++ b/src/core/system_parameters/gps_cnav_ephemeris.h @@ -47,8 +47,6 @@ public: } double delta_A{}; //!< Semi-major axis difference at reference time - double Adot{}; //!< Change rate in semi-major axis - double delta_ndot{}; //!< Rate of mean motion difference from computed value double delta_OMEGAdot{}; //!< Rate of Right Ascension difference [semi-circles/s] int32_t toe1{}; //!< Ephemeris data reference time of week (Ref. 20.3.3.4.3 IS-GPS-200M) [s] int32_t toe2{}; //!< Ephemeris data reference time of week (Ref. 20.3.3.4.3 IS-GPS-200M) [s] From edaedcc9b2110ae7774fa49c371e55547949967c Mon Sep 17 00:00:00 2001 From: Vladslav P Date: Tue, 28 Jul 2026 21:05:35 +0300 Subject: [PATCH 2/7] GPS CNAV: suggested corrections Set *fidx to zero at the beginning of prange function Move mean motion average estimation to mean anomaly calculation Use Ak to calculate rkdot Update mean anomaly calculation in Gnss_Ephemeris::sv_clock_relativistic_term Store delta_ndot to XML and bump BOOST_CLASS_VERSION to 2 Signed-off-by: Vladslav P --- src/algorithms/libs/rtklib/rtklib_pntpos.cc | 1 + src/core/system_parameters/gnss_ephemeris.cc | 10 +++++----- src/core/system_parameters/gps_cnav_ephemeris.h | 6 +++++- 3 files changed, 11 insertions(+), 6 deletions(-) diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index 099a39ab5..7cf21bae7 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -183,6 +183,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, int j = 1; int sys = satsys(obs->sat, nullptr); *var = 0.0; + *fidx = 0; if (sys == SYS_NONE) { diff --git a/src/core/system_parameters/gnss_ephemeris.cc b/src/core/system_parameters/gnss_ephemeris.cc index a518e7d17..249021d88 100644 --- a/src/core/system_parameters/gnss_ephemeris.cc +++ b/src/core/system_parameters/gnss_ephemeris.cc @@ -194,10 +194,10 @@ void Gnss_Ephemeris::satellitePosVelComputation(double transmitTime, std::array< const double Ak = a + this->Adot * tk; // Corrected mean motion - const double n = n0 + this->delta_n + 0.5 * this->delta_ndot * tk; + const double n = n0 + this->delta_n; // Mean anomaly - const double M = this->M_0 + n * tk; + const double M = this->M_0 + (n + 0.5 * this->delta_ndot * tk) * tk; // Initial guess of eccentric anomaly double E = M; @@ -221,7 +221,7 @@ void Gnss_Ephemeris::satellitePosVelComputation(double transmitTime, std::array< const double cek = cos(E); const double OneMinusecosE = 1.0 - this->ecc * cek; const double sq1e2 = sqrt(1.0 - this->ecc * this->ecc); - const double ekdot = n / OneMinusecosE; + const double ekdot = (n + this->delta_ndot * tk) / OneMinusecosE; // Compute the true anomaly const double tmp_Y = sq1e2 * sek; @@ -244,7 +244,7 @@ void Gnss_Ephemeris::satellitePosVelComputation(double transmitTime, std::array< // Correct radius const double r = Ak * OneMinusecosE + this->Crc * c2pk + this->Crs * s2pk; - const double rkdot = this->Adot * (1. - this->ecc * cek) + a * this->ecc * sek * ekdot + 2.0 * pkdot * (this->Crs * c2pk - this->Crc * s2pk); + const double rkdot = this->Adot * (1. - this->ecc * cek) + Ak * this->ecc * sek * ekdot + 2.0 * pkdot * (this->Crs * c2pk - this->Crc * s2pk); // Correct inclination const double i = this->i_0 + this->idot * tk + this->Cic * c2pk + this->Cis * s2pk; @@ -348,7 +348,7 @@ double Gnss_Ephemeris::sv_clock_relativistic_term(double transmitTime) const const double n = n0 + this->delta_n; // Mean anomaly - const double M = this->M_0 + n * tk; + const double M = this->M_0 + (n + 0.5 * this->delta_ndot * tk) * tk; // Initial guess of eccentric anomaly double E = M; diff --git a/src/core/system_parameters/gps_cnav_ephemeris.h b/src/core/system_parameters/gps_cnav_ephemeris.h index 1d5131726..8da03cd28 100644 --- a/src/core/system_parameters/gps_cnav_ephemeris.h +++ b/src/core/system_parameters/gps_cnav_ephemeris.h @@ -138,6 +138,10 @@ public: archive& BOOST_SERIALIZATION_NVP(URA1); archive& BOOST_SERIALIZATION_NVP(URA2); } + if (version > 1) + { + archive& BOOST_SERIALIZATION_NVP(delta_ndot); + } archive& BOOST_SERIALIZATION_NVP(TGD); archive& BOOST_SERIALIZATION_NVP(ISCL1); archive& BOOST_SERIALIZATION_NVP(ISCL2); @@ -153,7 +157,7 @@ public: } }; -BOOST_CLASS_VERSION(Gps_CNAV_Ephemeris, 1) +BOOST_CLASS_VERSION(Gps_CNAV_Ephemeris, 2) /** \} */ From a42d93567b38ab9d9be6511c9b80121594369174 Mon Sep 17 00:00:00 2001 From: Vladslav P Date: Thu, 30 Jul 2026 23:40:01 +0300 Subject: [PATCH 3/7] GnssEphemeris position/velocity test: CNAV Signed-off-by: Vladslav P --- tests/test_main.cc | 1 + .../gnss_ephemeris_posvel_test.cc | 101 ++++++++++++++++++ 2 files changed, 102 insertions(+) create mode 100644 tests/unit-tests/system-parameters/gnss_ephemeris_posvel_test.cc diff --git a/tests/test_main.cc b/tests/test_main.cc index f1a674668..492fb6c59 100644 --- a/tests/test_main.cc +++ b/tests/test_main.cc @@ -188,6 +188,7 @@ private: #include "unit-tests/system-parameters/galileo_ism_test.cc" #include "unit-tests/system-parameters/glonass_gnav_ephemeris_test.cc" #include "unit-tests/system-parameters/glonass_gnav_nav_message_test.cc" +#include "unit-tests/system-parameters/gnss_ephemeris_posvel_test.cc" #include "unit-tests/system-parameters/gps_cnav_navigation_message_test.cc" #include "unit-tests/system-parameters/has_decoding_test.cc" #include "unit-tests/system-parameters/qzss_code_generation_test.cc" diff --git a/tests/unit-tests/system-parameters/gnss_ephemeris_posvel_test.cc b/tests/unit-tests/system-parameters/gnss_ephemeris_posvel_test.cc new file mode 100644 index 000000000..8fa7582ad --- /dev/null +++ b/tests/unit-tests/system-parameters/gnss_ephemeris_posvel_test.cc @@ -0,0 +1,101 @@ +/*! + * \file gnss_ephemeris_posvel_test.cc.cc + * \author Vladislav P, 2026. vladisslav2011(at)gmail.com + * + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + + +#include "gnss_ephemeris.h" +#include "gps_cnav_ephemeris.h" + + +TEST(GnssEphemerisPosVelTest, CNAV) +{ + Gps_CNAV_Ephemeris e{}; + e.PRN = 1; + e.M_0 = 8.45797918146817151e-01; + e.delta_n = 4.55893989814592299e-09; + e.ecc = 1.83155917329713681e-03; + e.sqrtA = 5.15360200490880015e+03; + e.OMEGA_0 = 6.26039239835774475e-01; + e.i_0 = 9.57107201573828559e-01; + e.omega = 1.91825002944197659e-01; + e.OMEGAdot = -8.21850013985512873e-09; + e.idot = 1.84293390845179697e-10; + e.Cuc = 3.64799052476882935e-06; + e.Cus = 3.35834920406341553e-06; + e.Crc = 3.11757812500000000e+02; + e.Crs = 6.96562500000000000e+01; + e.Cic = 4.00468707084655762e-08; + e.Cis = 4.00468707084655762e-08; + e.toe = 0; + e.toc = 588600; + e.af0 = 2.13293504202738377e-04; + e.af1 = -9.59232693276135251e-12; + e.af2 = 0.00000000000000000e+00; + e.WN = 2428; + e.tow = 590328; + e.satClkDrift = 0.00000000000000000e+00; + e.dtr = 0.00000000000000000e+00; + e.toe1 = 588600; + e.toe2 = 588600; + e.WNop = 124; + e.top = 519300; + e.URAED = -1; + e.URANED0 = -2; + e.URANED1 = 3; + e.URANED2 = 7; + e.URA = -1; + e.URA0 = -2.00000000000000000e+00; + e.URA1 = 3.00000000000000000e+00; + e.URA2 = 7.00000000000000000e+00; + e.delta_ndot = -8.06351598383372435e-14; + e.TGD = -8.84756445884704424e-09; + e.ISCL1 = -2.91038304567336984e-10; + e.ISCL2 = 5.64614310860633767e-09; + e.ISCL5I = -5.52972778677940265e-10; + e.ISCL5Q = -6.69388100504875080e-10; + e.delta_A = -9.63750000000000000e+01; + e.Adot = 7.12776184082031250e-03; + e.delta_OMEGAdot = -5.03592405216479423e-11; + e.integrity_status_flag = 0; + e.l2c_phasing_flag = 0; + e.alert_flag = 0; + e.antispoofing_flag = 0; + auto dopplerL2 = e.predicted_doppler(590328., 0., 0., 0., 0., 0., 0., 2); + auto dopplerL5 = e.predicted_doppler(590328., 0., 0., 0., 0., 0., 0., 5); + e.satellitePosition(590328.); + EXPECT_NEAR(-411.339708, dopplerL2, 1e-4); + EXPECT_NEAR(-394.200554, dopplerL5, 1e-4); + EXPECT_NEAR(11987089.716145, e.satpos_X, 1e-4); + EXPECT_NEAR(13994848.326179, e.satpos_Y, 1e-4); + EXPECT_NEAR(-19106970.005881, e.satpos_Z, 1e-4); + EXPECT_NEAR(-411.226253, e.satvel_X, 1e-4); + EXPECT_NEAR( 2397.343969, e.satvel_Y, 1e-4); + EXPECT_NEAR(1507.249499, e.satvel_Z, 1e-4); + double satvel_X = e.satvel_X; + double satvel_Y = e.satvel_Y; + double satvel_Z = e.satvel_Z; + e.satellitePosition(590327.995); + double satpos_X = e.satpos_X; + double satpos_Y = e.satpos_Y; + double satpos_Z = e.satpos_Z; + e.satellitePosition(590328.005); + double d_X = (e.satpos_X - satpos_X) * 100.; + double d_Y = (e.satpos_Y - satpos_Y) * 100.; + double d_Z = (e.satpos_Z - satpos_Z) * 100.; + EXPECT_NEAR(d_X, satvel_X, 1e-4); + EXPECT_NEAR(d_Y, satvel_Y, 1e-4); + EXPECT_NEAR(d_Z, satvel_Z, 1e-4); +} + From 5eb08f9baa6b6c628cadf24952f459a9589769a2 Mon Sep 17 00:00:00 2001 From: Vladslav P Date: Thu, 30 Jul 2026 23:49:01 +0300 Subject: [PATCH 4/7] GnssEphemeris position/velocity test: GPS NAV Signed-off-by: Vladslav P --- .../gnss_ephemeris_posvel_test.cc | 71 ++++++++++++++++++- 1 file changed, 70 insertions(+), 1 deletion(-) diff --git a/tests/unit-tests/system-parameters/gnss_ephemeris_posvel_test.cc b/tests/unit-tests/system-parameters/gnss_ephemeris_posvel_test.cc index 8fa7582ad..ceaf4ec5a 100644 --- a/tests/unit-tests/system-parameters/gnss_ephemeris_posvel_test.cc +++ b/tests/unit-tests/system-parameters/gnss_ephemeris_posvel_test.cc @@ -14,9 +14,78 @@ * ----------------------------------------------------------------------------- */ - #include "gnss_ephemeris.h" #include "gps_cnav_ephemeris.h" +#include "gps_ephemeris.h" + + +TEST(GnssEphemerisPosVelTest, NAV) +{ + Gps_Ephemeris e{}; + e.PRN = 15; + e.M_0 = 1.35464234665685956e+00; + e.delta_n = 5.10199823299105835e-09; + e.ecc = 1.70680778101086582e-02; + e.sqrtA = 5.15376512145996094e+03; + e.OMEGA_0 = 2.35100387398234556e+00; + e.i_0 = 9.44768674001770559e-01; + e.omega = 1.54160131395114042e+00; + e.OMEGAdot = -8.57285709390860145e-09; + e.idot = -2.30366738556474118e-10; + e.Cuc = -6.73159956932067871e-06; + e.Cus = 3.66382300853729248e-06; + e.Crc = 3.05843750000000000e+02; + e.Crs = -1.23937500000000000e+02; + e.Cic = -1.17346644401550293e-07; + e.Cis = -3.91155481338500977e-07; + e.toe = 331200; + e.toc = 331200; + e.af0 = 4.32340428233146667e-04; + e.af1 = 3.06954461848363200e-12; + e.af2 = 0.00000000000000000e+00; + e.WN = 381; + e.tow = 326166; + e.satClkDrift = 0.00000000000000000e+00; + e.dtr = 0.00000000000000000e+00; + e.IODE_SF2 = 30; + e.IODE_SF3 = 30; + e.code_on_L2 = 1; + e.L2_P_data_flag = 0; + e.SV_accuracy = 0; + e.SV_health = 0; + e.TGD = -1.07102096080780029e-08; + e.IODC = 30; + e.AODO = 16200; + e.fit_interval_flag = 0; + e.spare1 = 0.00000000000000000e+00; + e.spare2 = 0.00000000000000000e+00; + e.integrity_status_flag = 0; + e.alert_flag = 0; + e.antispoofing_flag = 1; + auto dopplerL1 = e.predicted_doppler(590328., 0., 0., 0., 0., 0., 0., 1); + e.satellitePosition(590328.); + EXPECT_NEAR(98.676019, dopplerL1, 1e-4); + EXPECT_NEAR(26325247.164428, e.satpos_X, 1e-4); + EXPECT_NEAR(1943900.228289, e.satpos_Y, 1e-4); + EXPECT_NEAR(2494201.363945, e.satpos_Z, 1e-4); + EXPECT_NEAR(333.268032, e.satvel_X, 1e-4); + EXPECT_NEAR(384.414587, e.satvel_Y, 1e-4); + EXPECT_NEAR(-3116.921783, e.satvel_Z, 1e-4); + double satvel_X = e.satvel_X; + double satvel_Y = e.satvel_Y; + double satvel_Z = e.satvel_Z; + e.satellitePosition(590327.995); + double satpos_X = e.satpos_X; + double satpos_Y = e.satpos_Y; + double satpos_Z = e.satpos_Z; + e.satellitePosition(590328.005); + double d_X = (e.satpos_X - satpos_X) * 100.; + double d_Y = (e.satpos_Y - satpos_Y) * 100.; + double d_Z = (e.satpos_Z - satpos_Z) * 100.; + EXPECT_NEAR(d_X, satvel_X, 1e-4); + EXPECT_NEAR(d_Y, satvel_Y, 1e-4); + EXPECT_NEAR(d_Z, satvel_Z, 1e-4); +} TEST(GnssEphemerisPosVelTest, CNAV) From b070e4fbe4a5d96ae49cde428c9366264fd5efa6 Mon Sep 17 00:00:00 2001 From: Vladslav P Date: Fri, 31 Jul 2026 01:55:08 +0300 Subject: [PATCH 5/7] GnssEphemeris position/velocity test: Galileo Signed-off-by: Vladslav P --- .../gnss_ephemeris_posvel_test.cc | 75 +++++++++++++++++++ 1 file changed, 75 insertions(+) diff --git a/tests/unit-tests/system-parameters/gnss_ephemeris_posvel_test.cc b/tests/unit-tests/system-parameters/gnss_ephemeris_posvel_test.cc index ceaf4ec5a..5bf56a0c6 100644 --- a/tests/unit-tests/system-parameters/gnss_ephemeris_posvel_test.cc +++ b/tests/unit-tests/system-parameters/gnss_ephemeris_posvel_test.cc @@ -14,6 +14,7 @@ * ----------------------------------------------------------------------------- */ +#include "galileo_ephemeris.h" #include "gnss_ephemeris.h" #include "gps_cnav_ephemeris.h" #include "gps_ephemeris.h" @@ -168,3 +169,77 @@ TEST(GnssEphemerisPosVelTest, CNAV) EXPECT_NEAR(d_Z, satvel_Z, 1e-4); } + +TEST(GnssEphemerisPosVelTest, Galileo) +{ + Galileo_Ephemeris e{}; + e.PRN = 13; + e.M_0 = 2.41163546760016079e+00; + e.delta_n = 2.66832543217894277e-09; + e.ecc = 4.77104913443326882e-05; + e.sqrtA = 5.44060108375549316e+03; + e.OMEGA_0 = 6.47008588356165260e-01; + e.i_0 = 9.97650131401018214e-01; + e.omega = -7.14945752017439456e-02; + e.OMEGAdot = -5.50165773755647669e-09; + e.idot = 3.22870591713259829e-10; + e.Cuc = 6.63474202156066895e-06; + e.Cus = 2.07684934139251709e-06; + e.Crc = 3.11656250000000000e+02; + e.Crs = 1.40062500000000000e+02; + e.Cic = -2.79396772384643555e-08; + e.Cis = 4.47034835815429688e-08; + e.toe = 57000; + e.toc = 57000; + e.af0 = -6.03639055043458871e-05; + e.af1 = -9.37916411203332043e-13; + e.af2 = 0.00000000000000000e+00; + e.WN = 2429; + e.tow = 59670; + e.satClkDrift = 0.00000000000000000e+00; + e.dtr = 0.00000000000000000e+00; + e.IOD_ephemeris = 95; + e.IOD_nav = 0; + e.SISA = 107; + e.E5a_HS = 0; + e.E5b_HS = 0; + e.E1B_HS = 0; + e.E5a_DVS = 0; + e.E5b_DVS = 0; + e.E1B_DVS = 0; + e.BGD_E1E5a = 4.42378222942352212e-09; + e.BGD_E1E5b = 0.00000000000000000e+00; + e.flag_all_ephemeris = 1; + e.nav_message_type = static_cast(2); + auto dopplerL1 = e.predicted_doppler(59328., 0., 0., 0., 0., 0., 0., 1); + auto dopplerL5 = e.predicted_doppler(59328., 0., 0., 0., 0., 0., 0., 5); + auto dopplerL6 = e.predicted_doppler(59328., 0., 0., 0., 0., 0., 0., 6); + auto dopplerL7 = e.predicted_doppler(59328., 0., 0., 0., 0., 0., 0., 7); + auto dopplerL8 = e.predicted_doppler(59328., 0., 0., 0., 0., 0., 0., 8); + e.satellitePosition(59328.); + EXPECT_NEAR(1250.566176, dopplerL1, 1e-4); + EXPECT_NEAR(933.864352, dopplerL5, 1e-4); + EXPECT_NEAR(1015.069948, dopplerL6, 1e-4); + EXPECT_NEAR(958.226030, dopplerL7, 1e-4); + EXPECT_NEAR(946.045191, dopplerL8, 1e-4); + EXPECT_NEAR(18121174.847871, e.satpos_X, 1e-4); + EXPECT_NEAR(-19974436.330324, e.satpos_Y, 1e-4); + EXPECT_NEAR(12202101.756204, e.satpos_Z, 1e-4); + EXPECT_NEAR(977.807753, e.satvel_X, 1e-4); + EXPECT_NEAR(-754.332969, e.satvel_Y, 1e-4); + EXPECT_NEAR(-2686.561660, e.satvel_Z, 1e-4); + double satvel_X = e.satvel_X; + double satvel_Y = e.satvel_Y; + double satvel_Z = e.satvel_Z; + e.satellitePosition(59327.995); + double satpos_X = e.satpos_X; + double satpos_Y = e.satpos_Y; + double satpos_Z = e.satpos_Z; + e.satellitePosition(59328.005); + double d_X = (e.satpos_X - satpos_X) * 100.; + double d_Y = (e.satpos_Y - satpos_Y) * 100.; + double d_Z = (e.satpos_Z - satpos_Z) * 100.; + EXPECT_NEAR(d_X, satvel_X, 1e-4); + EXPECT_NEAR(d_Y, satvel_Y, 1e-4); + EXPECT_NEAR(d_Z, satvel_Z, 1e-4); +} From 2f86def08b1a763cda9d2379aa0d48c6f4ac5093 Mon Sep 17 00:00:00 2001 From: Vladslav P Date: Fri, 31 Jul 2026 02:34:45 +0300 Subject: [PATCH 6/7] GnssEphemeris position/velocity test: BeiDou DNAV Signed-off-by: Vladslav P --- .../gnss_ephemeris_posvel_test.cc | 74 ++++++++++++++++++- 1 file changed, 73 insertions(+), 1 deletion(-) diff --git a/tests/unit-tests/system-parameters/gnss_ephemeris_posvel_test.cc b/tests/unit-tests/system-parameters/gnss_ephemeris_posvel_test.cc index 5bf56a0c6..c5a26969a 100644 --- a/tests/unit-tests/system-parameters/gnss_ephemeris_posvel_test.cc +++ b/tests/unit-tests/system-parameters/gnss_ephemeris_posvel_test.cc @@ -14,6 +14,7 @@ * ----------------------------------------------------------------------------- */ +#include "beidou_dnav_ephemeris.h" #include "galileo_ephemeris.h" #include "gnss_ephemeris.h" #include "gps_cnav_ephemeris.h" @@ -151,7 +152,7 @@ TEST(GnssEphemerisPosVelTest, CNAV) EXPECT_NEAR(13994848.326179, e.satpos_Y, 1e-4); EXPECT_NEAR(-19106970.005881, e.satpos_Z, 1e-4); EXPECT_NEAR(-411.226253, e.satvel_X, 1e-4); - EXPECT_NEAR( 2397.343969, e.satvel_Y, 1e-4); + EXPECT_NEAR(2397.343969, e.satvel_Y, 1e-4); EXPECT_NEAR(1507.249499, e.satvel_Z, 1e-4); double satvel_X = e.satvel_X; double satvel_Y = e.satvel_Y; @@ -243,3 +244,74 @@ TEST(GnssEphemerisPosVelTest, Galileo) EXPECT_NEAR(d_Y, satvel_Y, 1e-4); EXPECT_NEAR(d_Z, satvel_Z, 1e-4); } + + +TEST(GnssEphemerisPosVelTest, DNAV) +{ + Beidou_Dnav_Ephemeris e{}; + e.PRN = 41; + e.M_0 = -2.86495395534618202e+00; + e.delta_n = 3.27227916070452066e-09; + e.ecc = 1.44056859426200368e-03; + e.sqrtA = 5.28263833618164062e+03; + e.OMEGA_0 = 2.17724435277226158e+00; + e.i_0 = 9.87684843163906256e-01; + e.omega = -1.04237488131494449e+00; + e.OMEGAdot = -6.49884213143194274e-09; + e.idot = -1.00004165574903490e-11; + e.Cuc = -7.40075483918190002e-06; + e.Cus = 5.91995194554328918e-06; + e.Crc = 2.55390625000000000e+02; + e.Crs = -1.52921875000000000e+02; + e.Cic = -1.07102096080780029e-08; + e.Cis = 3.44589352607727051e-08; + e.toe = 586800; + e.toc = 586800; + e.af0 = -9.49730747379362475e-04; + e.af1 = -2.21955787083061296e-12; + e.af2 = 0.00000000000000000e+00; + e.WN = 1072; + e.tow = 590232; + e.satClkDrift = 0.00000000000000000e+00; + e.dtr = 0.00000000000000000e+00; + e.AODE = 1.00000000000000000e+00; + e.SV_accuracy = 0; + e.SV_health = 0; + e.AODC = 1.00000000000000000e+00; + e.TGD1 = -2.39999999999999998e-08; + e.TGD2 = -2.39999999999999998e-08; + e.sig_type = 5; + e.nav_type = 1; + e.AODO = 0; + e.fit_interval_flag = 0; + e.spare1 = 0.00000000000000000e+00; + e.spare2 = 0.00000000000000000e+00; + e.integrity_status_flag = 0; + e.alert_flag = 0; + e.antispoofing_flag = 0; + auto dopplerL1 = e.predicted_doppler(590328., 0., 0., 0., 0., 0., 0., 1); + auto dopplerL3 = e.predicted_doppler(590328., 0., 0., 0., 0., 0., 0., 3); + e.satellitePosition(590328.); + EXPECT_NEAR(1168.881533, dopplerL1, 1e-4); + EXPECT_NEAR(949.811992, dopplerL3, 1e-4); + EXPECT_NEAR(26628945.935993, e.satpos_X, 1e-4); + EXPECT_NEAR(-5184093.916996, e.satpos_Y, 1e-4); + EXPECT_NEAR(6666187.976574, e.satpos_Z, 1e-4); + EXPECT_NEAR(755.905578, e.satvel_X, 1e-4); + EXPECT_NEAR(18.179881, e.satvel_Y, 1e-4); + EXPECT_NEAR(-3021.004498, e.satvel_Z, 1e-4); + double satvel_X = e.satvel_X; + double satvel_Y = e.satvel_Y; + double satvel_Z = e.satvel_Z; + e.satellitePosition(590327.995); + double satpos_X = e.satpos_X; + double satpos_Y = e.satpos_Y; + double satpos_Z = e.satpos_Z; + e.satellitePosition(590328.005); + double d_X = (e.satpos_X - satpos_X) * 100.; + double d_Y = (e.satpos_Y - satpos_Y) * 100.; + double d_Z = (e.satpos_Z - satpos_Z) * 100.; + EXPECT_NEAR(d_X, satvel_X, 1e-4); + EXPECT_NEAR(d_Y, satvel_Y, 1e-4); + EXPECT_NEAR(d_Z, satvel_Z, 1e-4); +} From 3fe0226f4bfca58d062d74530d6c57bf894aaf42 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 31 Jul 2026 17:17:23 +0200 Subject: [PATCH 7/7] Update changelog --- docs/CHANGELOG.md | 5 +++++ src/algorithms/PVT/libs/rtklib_solver.cc | 7 +++++++ 2 files changed, 12 insertions(+) diff --git a/docs/CHANGELOG.md b/docs/CHANGELOG.md index 8373d4c70..2590af434 100644 --- a/docs/CHANGELOG.md +++ b/docs/CHANGELOG.md @@ -46,6 +46,11 @@ All notable changes to GNSS-SDR will be documented in this file. and 1.79 for L5/E5a), and its variance by the square of that factor. Previously the unscaled L1 delay was applied to those measurements, undercorrecting the ionosphere by the same factor. +- Improved PVT processing of GPS L2C, GPS L5, and QZSS signals using CNAV + navigation data: satellite positions now include the CNAV semi-major axis and + mean-motion rate terms, and group-delay / inter-signal corrections follow + IS-GPS-200 / IS-GPS-705 in both single-band and L1+L5 dual-band + configurations. Contributed by @vladisslav2011. ### Improvements in Availability: diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index f9ca62c6b..fb33810f8 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1653,6 +1653,13 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ if (eph_data[i].sat == sat) { // 2. If found, attach the L2/L5 observation to the existing observation in RTKLIB structure + // The existing entry carries the LNAV ephemeris, which has no + // inter-signal corrections: take the ISCs from CNAV so the + // dual-frequency correction in prange() can apply them. + eph_data[i].isc[0] = gps_cnav_ephemeris_iter->second.ISCL1; + eph_data[i].isc[1] = gps_cnav_ephemeris_iter->second.ISCL2; + eph_data[i].isc[2] = gps_cnav_ephemeris_iter->second.ISCL5I; + eph_data[i].isc[3] = gps_cnav_ephemeris_iter->second.ISCL5Q; if (eph_data[i].apply_has_corrections) { const HAS_obs_corrections *applied_has_correction = nullptr;