mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-01-19 05:33:02 +00:00
Code Issues Releases Wiki Activity

Improve computation of satellite position and velocity in ephemeris classes

Browse Source
This commit is contained in:
Carles Fernandez 2020-11-29 12:08:23 +01:00
parent c9dc767c96
commit 38cd7237dc
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
5 changed files with 189 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
src/core/system_parameters/beidou_dnav_ephemeris.cc
View File

@ -149,41 +149,65 @@ double Beidou_Dnav_Ephemeris::satellitePosition(double transmitTime)
} }
// Compute the true anomaly // Compute the true anomaly
const double tmp_Y = sqrt(1.0 - d_eccentricity * d_eccentricity) * sin(E); const double sek = sin(E);
const double tmp_X = cos(E) - d_eccentricity; const double cek = cos(E);
const double OneMinusecosE = 1.0 - d_eccentricity * cek;
const double ekdot = n / OneMinusecosE;
// Compute the true anomaly
const double sq1e2 = sqrt(1.0 - d_eccentricity * d_eccentricity);
const double tmp_Y = sq1e2 * sek;
const double tmp_X = cek - d_eccentricity;
const double nu = atan2(tmp_Y, tmp_X); const double nu = atan2(tmp_Y, tmp_X);
// Compute angle phi (argument of Latitude) // Compute angle phi (argument of Latitude)
double phi = nu + d_OMEGA; const double phi = nu + d_OMEGA;
double pkdot = sq1e2 * ekdot / OneMinusecosE;
// Reduce phi to between 0 and 2*pi rad // Reduce phi to between 0 and 2*pi rad
phi = fmod((phi), (2.0 * GNSS_PI)); // phi = fmod((phi), (2.0 * GNSS_PI));
const double s2pk = sin(2.0 * phi);
const double c2pk = cos(2.0 * phi);
// Correct argument of latitude // Correct argument of latitude
const double u = phi + d_Cuc * cos(2.0 * phi) + d_Cus * sin(2.0 * phi); const double u = phi + d_Cuc * c2pk + d_Cus * s2pk;
const double cuk = cos(u);
const double suk = sin(u);
const double ukdot = pkdot * (1.0 + 2.0 * (d_Cus * c2pk - d_Cuc * s2pk));
// Correct radius // Correct radius
const double r = a * (1.0 - d_eccentricity * cos(E)) + d_Crc * cos(2.0 * phi) + d_Crs * sin(2.0 * phi); const double r = a * (1.0 - d_eccentricity * cek) + d_Crc * c2pk + d_Crs * s2pk;
const double rkdot = a * d_eccentricity * sek * ekdot + 2.0 * pkdot * (d_Crs * c2pk - d_Crc * s2pk);
// Correct inclination // Correct inclination
const double i = d_i_0 + d_IDOT * tk + d_Cic * cos(2.0 * phi) + d_Cis * sin(2.0 * phi); const double i = d_i_0 + d_IDOT * tk + d_Cic * c2pk + d_Cis * s2pk;
const double sik = sin(i);
const double cik = cos(i);
const double ikdot = d_IDOT + 2.0 * pkdot * (d_Cis * c2pk - d_Cic * s2pk);
// Compute the angle between the ascending node and the Greenwich meridian // Compute the angle between the ascending node and the Greenwich meridian
double Omega = d_OMEGA0 + (d_OMEGA_DOT - BEIDOU_OMEGA_EARTH_DOT) * tk - BEIDOU_OMEGA_EARTH_DOT * d_Toe; const double Omega = d_OMEGA0 + (d_OMEGA_DOT - BEIDOU_OMEGA_EARTH_DOT) * tk - BEIDOU_OMEGA_EARTH_DOT * d_Toe;
const double sok = sin(Omega);
// Reduce to between 0 and 2*pi rad const double cok = cos(Omega);
Omega = fmod((Omega + 2.0 * GNSS_PI), (2.0 * GNSS_PI));
// --- Compute satellite coordinates in Earth-fixed coordinates // --- Compute satellite coordinates in Earth-fixed coordinates
d_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega); const double xprime = r * cuk;
d_satpos_Y = cos(u) * r * sin(Omega) + sin(u) * r * cos(i) * cos(Omega); const double yprime = r * suk;
d_satpos_Z = sin(u) * r * sin(i); d_satpos_X = xprime * cok - yprime * cik * sok;
d_satpos_Y = xprime * sok + yprime * cik * cok;
d_satpos_Z = yprime * sik;
// Satellite's velocity. Can be useful for Vector Tracking loops // Satellite's velocity. Can be useful for Vector Tracking loops
const double Omega_dot = d_OMEGA_DOT - BEIDOU_OMEGA_EARTH_DOT; const double Omega_dot = d_OMEGA_DOT - BEIDOU_OMEGA_EARTH_DOT;
d_satvel_X = -Omega_dot * (cos(u) * r + sin(u) * r * cos(i)) + d_satpos_X * cos(Omega) - d_satpos_Y * cos(i) * sin(Omega);
d_satvel_Y = Omega_dot * (cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega)) + d_satpos_X * sin(Omega) + d_satpos_Y * cos(i) * cos(Omega); const double xpkdot = rkdot * cuk - yprime * ukdot;
d_satvel_Z = d_satpos_Y * sin(i); const double ypkdot = rkdot * suk + xprime * ukdot;
const double tmp = ypkdot * cik - d_satpos_Z * ikdot;
d_satvel_X = -Omega_dot * d_satpos_Y + xpkdot * cok - tmp * sok;
d_satvel_Y = Omega_dot * d_satpos_X + xpkdot * sok + tmp * cok;
d_satvel_Z = yprime * cik * ikdot + ypkdot * sik;
// Time from ephemeris reference clock // Time from ephemeris reference clock
tk = check_t(transmitTime - d_Toc); tk = check_t(transmitTime - d_Toc);
@ -191,7 +215,7 @@ double Beidou_Dnav_Ephemeris::satellitePosition(double transmitTime)
double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk; double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk;
/* relativity correction */ /* relativity correction */
dtr_s -= 2.0 * sqrt(BEIDOU_GM * a) * d_eccentricity * sin(E) / (SPEED_OF_LIGHT_M_S * SPEED_OF_LIGHT_M_S); dtr_s -= 2.0 * sqrt(BEIDOU_GM * a) * d_eccentricity * sek / (SPEED_OF_LIGHT_M_S * SPEED_OF_LIGHT_M_S);
return dtr_s; return dtr_s;
} }

82
src/core/system_parameters/galileo_ephemeris.cc
View File

@ -78,7 +78,7 @@ double Galileo_Ephemeris::sv_clock_relativistic_term(double transmitTime) // Sa
// Time from ephemeris reference epoch // Time from ephemeris reference epoch
// t = WN_5*86400*7 + TOW_5; //WN_5*86400*7 are the second from the origin of the Galileo time // t = WN_5*86400*7 + TOW_5; //WN_5*86400*7 are the second from the origin of the Galileo time
const double tk = transmitTime - t0e_1; const double tk = check_t(transmitTime - static_cast<double>(t0e_1));
// Corrected mean motion // Corrected mean motion
const double n = n0 + delta_n_3; const double n = n0 + delta_n_3;
@ -87,7 +87,7 @@ double Galileo_Ephemeris::sv_clock_relativistic_term(double transmitTime) // Sa
double M = M0_1 + n * tk; double M = M0_1 + n * tk;
// Reduce mean anomaly to between 0 and 2pi // Reduce mean anomaly to between 0 and 2pi
M = fmod((M + 2 * GNSS_PI), (2 * GNSS_PI)); M = fmod((M + 2.0 * GNSS_PI), (2.0 * GNSS_PI));
// Initial guess of eccentric anomaly // Initial guess of eccentric anomaly
double E = M; double E = M;
@ -99,7 +99,7 @@ double Galileo_Ephemeris::sv_clock_relativistic_term(double transmitTime) // Sa
{ {
E_old = E; E_old = E;
E = M + e_1 * sin(E); E = M + e_1 * sin(E);
dE = fmod(E - E_old, 2 * GNSS_PI); dE = fmod(E - E_old, 2.0 * GNSS_PI);
if (fabs(dE) < 1e-12) if (fabs(dE) < 1e-12)
{ {
// Necessary precision is reached, exit from the loop // Necessary precision is reached, exit from the loop
@ -127,7 +127,7 @@ void Galileo_Ephemeris::satellitePosition(double transmitTime)
const double n0 = sqrt(GALILEO_GM / (a * a * a)); const double n0 = sqrt(GALILEO_GM / (a * a * a));
// Time from ephemeris reference epoch // Time from ephemeris reference epoch
const double tk = transmitTime - t0e_1; const double tk = check_t(transmitTime - static_cast<double>(t0e_1));
// Corrected mean motion // Corrected mean motion
const double n = n0 + delta_n_3; const double n = n0 + delta_n_3;
@ -136,7 +136,7 @@ void Galileo_Ephemeris::satellitePosition(double transmitTime)
double M = M0_1 + n * tk; double M = M0_1 + n * tk;
// Reduce mean anomaly to between 0 and 2pi // Reduce mean anomaly to between 0 and 2pi
M = fmod((M + 2 * GNSS_PI), (2 * GNSS_PI)); M = fmod((M + 2.0 * GNSS_PI), (2.0 * GNSS_PI));
// Initial guess of eccentric anomaly // Initial guess of eccentric anomaly
double E = M; double E = M;
@ -148,7 +148,7 @@ void Galileo_Ephemeris::satellitePosition(double transmitTime)
{ {
E_old = E; E_old = E;
E = M + e_1 * sin(E); E = M + e_1 * sin(E);
dE = fmod(E - E_old, 2 * GNSS_PI); dE = fmod(E - E_old, 2.0 * GNSS_PI);
if (fabs(dE) < 1e-12) if (fabs(dE) < 1e-12)
{ {
// Necessary precision is reached, exit from the loop // Necessary precision is reached, exit from the loop
@ -156,40 +156,80 @@ void Galileo_Ephemeris::satellitePosition(double transmitTime)
} }
} }
const double sek = sin(E);
const double cek = cos(E);
const double OneMinusecosE = 1.0 - e_1 * cek;
const double sq1e2 = sqrt(1.0 - e_1 * e_1);
const double ekdot = n / OneMinusecosE;
// Compute the true anomaly // Compute the true anomaly
const double tmp_Y = sqrt(1.0 - e_1 * e_1) * sin(E); const double tmp_Y = sq1e2 * sek;
const double tmp_X = cos(E) - e_1; const double tmp_X = cek - e_1;
const double nu = atan2(tmp_Y, tmp_X); const double nu = atan2(tmp_Y, tmp_X);
// Compute angle phi (argument of Latitude) // Compute angle phi (argument of Latitude)
double phi = nu + omega_2; double phi = nu + omega_2;
// Reduce phi to between 0 and 2*pi rad // Reduce phi to between 0 and 2*pi rad
phi = fmod((phi), (2 * GNSS_PI)); phi = fmod((phi), (2.0 * GNSS_PI));
const double s2pk = sin(2.0 * phi);
const double c2pk = cos(2.0 * phi);
const double pkdot = sq1e2 * ekdot / OneMinusecosE;
// Correct argument of latitude // Correct argument of latitude
const double u = phi + C_uc_3 * cos(2 * phi) + C_us_3 * sin(2 * phi); const double u = phi + C_uc_3 * c2pk + C_us_3 * s2pk;
const double suk = sin(u);
const double cuk = cos(u);
const double ukdot = pkdot * (1.0 + 2.0 * (C_us_3 * c2pk - C_uc_3 * s2pk));
// Correct radius // Correct radius
const double r = a * (1 - e_1 * cos(E)) + C_rc_3 * cos(2 * phi) + C_rs_3 * sin(2 * phi); const double r = a * OneMinusecosE + C_rc_3 * c2pk + C_rs_3 * s2pk;
const double rkdot = a * e_1 * sek * ekdot + 2.0 * pkdot * (C_rs_3 * c2pk - C_rc_3 * s2pk);
// Correct inclination // Correct inclination
const double i = i_0_2 + iDot_2 * tk + C_ic_4 * cos(2 * phi) + C_is_4 * sin(2 * phi); const double i = i_0_2 + iDot_2 * tk + C_ic_4 * c2pk + C_is_4 * s2pk;
const double sik = sin(i);
const double cik = cos(i);
const double ikdot = iDot_2 + 2.0 * pkdot * (C_is_4 * c2pk - C_ic_4 * s2pk);
// Compute the angle between the ascending node and the Greenwich meridian // Compute the angle between the ascending node and the Greenwich meridian
double Omega = OMEGA_0_2 + (OMEGA_dot_3 - GNSS_OMEGA_EARTH_DOT) * tk - GNSS_OMEGA_EARTH_DOT * t0e_1; const double Omega_dot = OMEGA_dot_3 - GNSS_OMEGA_EARTH_DOT;
double Omega = OMEGA_0_2 + Omega_dot * tk - GNSS_OMEGA_EARTH_DOT * static_cast<double>(t0e_1);
// Reduce to between 0 and 2*pi rad // Reduce to between 0 and 2*pi rad
Omega = fmod((Omega + 2 * GNSS_PI), (2 * GNSS_PI)); Omega = fmod((Omega + 2.0 * GNSS_PI), (2.0 * GNSS_PI));
const double sok = sin(Omega);
const double cok = cos(Omega);
// --- Compute satellite coordinates in Earth-fixed coordinates // --- Compute satellite coordinates in Earth-fixed coordinates
d_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega); const double xprime = r * cuk;
d_satpos_Y = cos(u) * r * sin(Omega) + sin(u) * r * cos(i) * cos(Omega); // ********NOTE: in GALILEO ICD this expression is not correct because it has minus (- sin(u) * r * cos(i) * cos(Omega)) instead of plus const double yprime = r * suk;
d_satpos_Z = sin(u) * r * sin(i); d_satpos_X = xprime * cok - yprime * cik * sok;
d_satpos_Y = xprime * sok + yprime * cik * cok; // ********NOTE: in GALILEO ICD this expression is not correct because it has minus (- sin(u) * r * cos(i) * cos(Omega)) instead of plus
d_satpos_Z = yprime * sik;
// Satellite's velocity. Can be useful for Vector Tracking loops // Satellite's velocity. Can be useful for Vector Tracking loops
const double Omega_dot = OMEGA_dot_3 - GNSS_OMEGA_EARTH_DOT; const double xpkdot = rkdot * cuk - yprime * ukdot;
d_satvel_X = -Omega_dot * (cos(u) * r + sin(u) * r * cos(i)) + d_satpos_X * cos(Omega) - d_satpos_Y * cos(i) * sin(Omega); const double ypkdot = rkdot * suk + xprime * ukdot;
d_satvel_Y = Omega_dot * (cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega)) + d_satpos_X * sin(Omega) + d_satpos_Y * cos(i) * cos(Omega); const double tmp = ypkdot * cik - d_satpos_Z * ikdot;
d_satvel_Z = d_satpos_Y * sin(i);
d_satvel_X = -Omega_dot * d_satpos_Y + xpkdot * cok - tmp * sok;
d_satvel_Y = Omega_dot * d_satpos_X + xpkdot * sok + tmp * cok;
d_satvel_Z = yprime * cik * ikdot + ypkdot * sik;
}
double Galileo_Ephemeris::check_t(double time)
{
const double half_week = 302400.0; // seconds
double corrTime = time;
if (time > half_week)
{
corrTime = time - 2.0 * half_week;
}
else if (time < -half_week)
{
corrTime = time + 2.0 * half_week;
}
return corrTime;
} }

8
src/core/system_parameters/galileo_ephemeris.h
View File

@ -162,6 +162,14 @@ public:
archive& BOOST_SERIALIZATION_NVP(flag_all_ephemeris); archive& BOOST_SERIALIZATION_NVP(flag_all_ephemeris);
} }
private:
/*
* Accounts for the beginning or end of week crossover
*
* \param[in] - time in seconds
* \param[out] - corrected time, in seconds
*/
double check_t(double time);
}; };

57
src/core/system_parameters/gps_cnav_ephemeris.cc
View File

@ -148,43 +148,66 @@ double Gps_CNAV_Ephemeris::satellitePosition(double transmitTime)
} }
} }
const double sek = sin(E);
const double cek = cos(E);
const double OneMinusecosE = 1.0 - d_e_eccentricity * cek;
const double ekdot = n / OneMinusecosE;
// Compute the true anomaly // Compute the true anomaly
const double tmp_Y = sqrt(1.0 - d_e_eccentricity * d_e_eccentricity) * sin(E); const double sq1e2 = sqrt(1.0 - d_e_eccentricity * d_e_eccentricity);
const double tmp_X = cos(E) - d_e_eccentricity; const double tmp_Y = sq1e2 * sek;
const double tmp_X = cek - d_e_eccentricity;
const double nu = atan2(tmp_Y, tmp_X); const double nu = atan2(tmp_Y, tmp_X);
// Compute angle phi (argument of Latitude) // Compute angle phi (argument of Latitude)
const double phi = nu + d_OMEGA; const double phi = nu + d_OMEGA;
double pkdot = sq1e2 * ekdot / OneMinusecosE;
// Reduce phi to between 0 and 2*pi rad // Reduce phi to between 0 and 2*pi rad
// phi = fmod((phi), (2*GNSS_PI)); // phi = fmod((phi), (2.0 * GNSS_PI));
const double s2pk = sin(2.0 * phi);
const double c2pk = cos(2.0 * phi);
// Correct argument of latitude // Correct argument of latitude
const double u = phi + d_Cuc * cos(2 * phi) + d_Cus * sin(2 * phi); const double u = phi + d_Cuc * c2pk + d_Cus * s2pk;
const double cuk = cos(u);
const double suk = sin(u);
const double ukdot = pkdot * (1.0 + 2.0 * (d_Cus * c2pk - d_Cuc * s2pk));
// Correct radius // Correct radius
const double r = a * (1 - d_e_eccentricity * cos(E)) + d_Crc * cos(2 * phi) + d_Crs * sin(2 * phi); const double r = a * (1.0 - d_e_eccentricity * cek) + d_Crc * c2pk + d_Crs * s2pk;
const double rkdot = a * d_e_eccentricity * sek * ekdot + 2.0 * pkdot * (d_Crs * c2pk - d_Crc * s2pk);
// Correct inclination // Correct inclination
const double i = d_i_0 + d_IDOT * tk + d_Cic * cos(2 * phi) + d_Cis * sin(2 * phi); const double i = d_i_0 + d_IDOT * tk + d_Cic * c2pk + d_Cis * s2pk;
const double sik = sin(i);
const double cik = cos(i);
const double ikdot = d_IDOT + 2.0 * pkdot * (d_Cis * c2pk - d_Cic * s2pk);
// Compute the angle between the ascending node and the Greenwich meridian // Compute the angle between the ascending node and the Greenwich meridian
const double d_OMEGA_DOT = OMEGA_DOT_REF * GNSS_PI + d_DELTA_OMEGA_DOT; const double d_OMEGA_DOT = OMEGA_DOT_REF * GNSS_PI + d_DELTA_OMEGA_DOT;
const double Omega = d_OMEGA0 + (d_OMEGA_DOT - GNSS_OMEGA_EARTH_DOT) * tk - GNSS_OMEGA_EARTH_DOT * d_Toe1; const double Omega = d_OMEGA0 + (d_OMEGA_DOT - GNSS_OMEGA_EARTH_DOT) * tk - GNSS_OMEGA_EARTH_DOT * d_Toe1;
const double sok = sin(Omega);
const double cok = cos(Omega);
// Reduce to between 0 and 2*pi rad // Compute satellite coordinates in Earth-fixed coordinates
// Omega = fmod((Omega + 2*GNSS_PI), (2*GNSS_PI)); const double xprime = r * cuk;
const double yprime = r * suk;
// --- Compute satellite coordinates in Earth-fixed coordinates d_satpos_X = xprime * cok - yprime * cik * sok;
d_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega); d_satpos_Y = xprime * sok + yprime * cik * cok;
d_satpos_Y = cos(u) * r * sin(Omega) + sin(u) * r * cos(i) * cos(Omega); d_satpos_Z = yprime * sik;
d_satpos_Z = sin(u) * r * sin(i);
// Satellite's velocity. Can be useful for Vector Tracking loops // Satellite's velocity. Can be useful for Vector Tracking loops
const double Omega_dot = d_OMEGA_DOT - GNSS_OMEGA_EARTH_DOT; const double Omega_dot = d_OMEGA_DOT - GNSS_OMEGA_EARTH_DOT;
d_satvel_X = -Omega_dot * (cos(u) * r + sin(u) * r * cos(i)) + d_satpos_X * cos(Omega) - d_satpos_Y * cos(i) * sin(Omega);
d_satvel_Y = Omega_dot * (cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega)) + d_satpos_X * sin(Omega) + d_satpos_Y * cos(i) * cos(Omega); const double xpkdot = rkdot * cuk - yprime * ukdot;
d_satvel_Z = d_satpos_Y * sin(i); const double ypkdot = rkdot * suk + xprime * ukdot;
const double tmp = ypkdot * cik - d_satpos_Z * ikdot;
d_satvel_X = -Omega_dot * d_satpos_Y + xpkdot * cok - tmp * sok;
d_satvel_Y = Omega_dot * d_satpos_X + xpkdot * sok + tmp * cok;
d_satvel_Z = yprime * cik * ikdot + ypkdot * sik;
// Time from ephemeris reference clock // Time from ephemeris reference clock
tk = check_t(transmitTime - d_Toc); tk = check_t(transmitTime - d_Toc);
@ -192,7 +215,7 @@ double Gps_CNAV_Ephemeris::satellitePosition(double transmitTime)
double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk; double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk;
/* relativity correction */ /* relativity correction */
dtr_s -= 2.0 * sqrt(GPS_GM * a) * d_e_eccentricity * sin(E) / (SPEED_OF_LIGHT_M_S * SPEED_OF_LIGHT_M_S); dtr_s -= 2.0 * sqrt(GPS_GM * a) * d_e_eccentricity * sek / (SPEED_OF_LIGHT_M_S * SPEED_OF_LIGHT_M_S);
return dtr_s; return dtr_s;
} }

53
src/core/system_parameters/gps_ephemeris.cc
View File

@ -155,42 +155,65 @@ double Gps_Ephemeris::satellitePosition(double transmitTime)
} }
} }
const double sek = sin(E);
const double cek = cos(E);
const double OneMinusecosE = 1.0 - d_e_eccentricity * cek;
const double ekdot = n / OneMinusecosE;
// Compute the true anomaly // Compute the true anomaly
const double tmp_Y = sqrt(1.0 - d_e_eccentricity * d_e_eccentricity) * sin(E); const double sq1e2 = sqrt(1.0 - d_e_eccentricity * d_e_eccentricity);
const double tmp_X = cos(E) - d_e_eccentricity; const double tmp_Y = sq1e2 * sek;
const double tmp_X = cek - d_e_eccentricity;
const double nu = atan2(tmp_Y, tmp_X); const double nu = atan2(tmp_Y, tmp_X);
// Compute angle phi (argument of Latitude) // Compute angle phi (argument of Latitude)
const double phi = nu + d_OMEGA; const double phi = nu + d_OMEGA;
double pkdot = sq1e2 * ekdot / OneMinusecosE;
// Reduce phi to between 0 and 2*pi rad // Reduce phi to between 0 and 2*pi rad
// phi = fmod((phi), (2.0 * GNSS_PI)); // phi = fmod((phi), (2.0 * GNSS_PI));
const double s2pk = sin(2.0 * phi);
const double c2pk = cos(2.0 * phi);
// Correct argument of latitude // Correct argument of latitude
const double u = phi + d_Cuc * cos(2.0 * phi) + d_Cus * sin(2.0 * phi); const double u = phi + d_Cuc * c2pk + d_Cus * s2pk;
const double cuk = cos(u);
const double suk = sin(u);
const double ukdot = pkdot * (1.0 + 2.0 * (d_Cus * c2pk - d_Cuc * s2pk));
// Correct radius // Correct radius
const double r = a * (1.0 - d_e_eccentricity * cos(E)) + d_Crc * cos(2.0 * phi) + d_Crs * sin(2.0 * phi); const double r = a * (1.0 - d_e_eccentricity * cek) + d_Crc * c2pk + d_Crs * s2pk;
const double rkdot = a * d_e_eccentricity * sek * ekdot + 2.0 * pkdot * (d_Crs * c2pk - d_Crc * s2pk);
// Correct inclination // Correct inclination
const double i = d_i_0 + d_IDOT * tk + d_Cic * cos(2.0 * phi) + d_Cis * sin(2.0 * phi); const double i = d_i_0 + d_IDOT * tk + d_Cic * c2pk + d_Cis * s2pk;
const double sik = sin(i);
const double cik = cos(i);
const double ikdot = d_IDOT + 2.0 * pkdot * (d_Cis * c2pk - d_Cic * s2pk);
// Compute the angle between the ascending node and the Greenwich meridian // Compute the angle between the ascending node and the Greenwich meridian
const double Omega = d_OMEGA0 + (d_OMEGA_DOT - GNSS_OMEGA_EARTH_DOT) * tk - GNSS_OMEGA_EARTH_DOT * d_Toe; const double Omega = d_OMEGA0 + (d_OMEGA_DOT - GNSS_OMEGA_EARTH_DOT) * tk - GNSS_OMEGA_EARTH_DOT * d_Toe;
const double sok = sin(Omega);
// Reduce to between 0 and 2*pi rad const double cok = cos(Omega);
// Omega = fmod((Omega + 2.0 * GNSS_PI), (2.0 * GNSS_PI));
// --- Compute satellite coordinates in Earth-fixed coordinates // --- Compute satellite coordinates in Earth-fixed coordinates
d_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega); const double xprime = r * cuk;
d_satpos_Y = cos(u) * r * sin(Omega) + sin(u) * r * cos(i) * cos(Omega); const double yprime = r * suk;
d_satpos_Z = sin(u) * r * sin(i); d_satpos_X = xprime * cok - yprime * cik * sok;
d_satpos_Y = xprime * sok + yprime * cik * cok;
d_satpos_Z = yprime * sik;
// Satellite's velocity. Can be useful for Vector Tracking loops // Satellite's velocity. Can be useful for Vector Tracking loops
const double Omega_dot = d_OMEGA_DOT - GNSS_OMEGA_EARTH_DOT; const double Omega_dot = d_OMEGA_DOT - GNSS_OMEGA_EARTH_DOT;
d_satvel_X = -Omega_dot * (cos(u) * r + sin(u) * r * cos(i)) + d_satpos_X * cos(Omega) - d_satpos_Y * cos(i) * sin(Omega);
d_satvel_Y = Omega_dot * (cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega)) + d_satpos_X * sin(Omega) + d_satpos_Y * cos(i) * cos(Omega); const double xpkdot = rkdot * cuk - yprime * ukdot;
d_satvel_Z = d_satpos_Y * sin(i); const double ypkdot = rkdot * suk + xprime * ukdot;
const double tmp = ypkdot * cik - d_satpos_Z * ikdot;
d_satvel_X = -Omega_dot * d_satpos_Y + xpkdot * cok - tmp * sok;
d_satvel_Y = Omega_dot * d_satpos_X + xpkdot * sok + tmp * cok;
d_satvel_Z = yprime * cik * ikdot + ypkdot * sik;
// Time from ephemeris reference clock // Time from ephemeris reference clock
tk = check_t(transmitTime - d_Toc); tk = check_t(transmitTime - d_Toc);
@ -198,7 +221,7 @@ double Gps_Ephemeris::satellitePosition(double transmitTime)
double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk; double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk;
/* relativity correction */ /* relativity correction */
dtr_s -= 2.0 * sqrt(GPS_GM * a) * d_e_eccentricity * sin(E) / (SPEED_OF_LIGHT_M_S * SPEED_OF_LIGHT_M_S); dtr_s -= 2.0 * sqrt(GPS_GM * a) * d_e_eccentricity * sek / (SPEED_OF_LIGHT_M_S * SPEED_OF_LIGHT_M_S);
return dtr_s; return dtr_s;
} }