mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2026-10-11 10:11:50 +00:00
Code Issues Releases Wiki Activity

GPS CNAV: fix satellite position computation

Browse Source 
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 <vladisslav2011@gmail.com>
This commit is contained in:
Vladslav P
2026-07-28 14:06:36 +03:00
parent cb2c955151
commit 60501b53fa
6 changed files with 46 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/algorithms/libs/rtklib/rtklib_ephemeris.cc
+8 -2
View File
@@ -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);
src/algorithms/libs/rtklib/rtklib_pntpos.cc
+12 -5
View File
@@ -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);
}
src/algorithms/libs/rtklib/rtklib_pntpos.h
+1 -1
View File
@@ -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
src/core/system_parameters/gnss_ephemeris.cc
+6 -3
View File
@@ -190,8 +190,11 @@ void Gnss_Ephemeris::satellitePosVelComputation(double transmitTime, std::array<
// Time from ephemeris reference epoch
double tk = check_t(transmitTime - static_cast<double>(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;
src/core/system_parameters/gnss_ephemeris.h
+19 -17
View File
@@ -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]
src/core/system_parameters/gps_cnav_ephemeris.h
-2
View File
@@ -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]