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]