From 38cd7237dc7b8ab6ca424114543b029372bf26b1 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sun, 29 Nov 2020 12:08:23 +0100 Subject: [PATCH] Improve computation of satellite position and velocity in ephemeris classes --- .../beidou_dnav_ephemeris.cc | 60 ++++++++++---- .../system_parameters/galileo_ephemeris.cc | 82 ++++++++++++++----- .../system_parameters/galileo_ephemeris.h | 8 ++ .../system_parameters/gps_cnav_ephemeris.cc | 57 +++++++++---- src/core/system_parameters/gps_ephemeris.cc | 53 ++++++++---- 5 files changed, 189 insertions(+), 71 deletions(-) diff --git a/src/core/system_parameters/beidou_dnav_ephemeris.cc b/src/core/system_parameters/beidou_dnav_ephemeris.cc index b78219202..b59c6dc3f 100644 --- a/src/core/system_parameters/beidou_dnav_ephemeris.cc +++ b/src/core/system_parameters/beidou_dnav_ephemeris.cc @@ -149,41 +149,65 @@ double Beidou_Dnav_Ephemeris::satellitePosition(double transmitTime) } // Compute the true anomaly - const double tmp_Y = sqrt(1.0 - d_eccentricity * d_eccentricity) * sin(E); - const double tmp_X = cos(E) - d_eccentricity; + const double sek = sin(E); + 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); // 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 - 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 - 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 - 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 - 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 - double Omega = d_OMEGA0 + (d_OMEGA_DOT - BEIDOU_OMEGA_EARTH_DOT) * tk - BEIDOU_OMEGA_EARTH_DOT * d_Toe; - - // Reduce to between 0 and 2*pi rad - Omega = fmod((Omega + 2.0 * GNSS_PI), (2.0 * GNSS_PI)); + const double Omega = d_OMEGA0 + (d_OMEGA_DOT - BEIDOU_OMEGA_EARTH_DOT) * tk - BEIDOU_OMEGA_EARTH_DOT * d_Toe; + const double sok = sin(Omega); + const double cok = cos(Omega); // --- Compute satellite coordinates in Earth-fixed coordinates - d_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega); - d_satpos_Y = cos(u) * r * sin(Omega) + sin(u) * r * cos(i) * cos(Omega); - d_satpos_Z = sin(u) * r * sin(i); + const double xprime = r * cuk; + const double yprime = r * suk; + 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 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); - d_satvel_Z = d_satpos_Y * sin(i); + + const double xpkdot = rkdot * cuk - yprime * ukdot; + 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 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; /* 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; } diff --git a/src/core/system_parameters/galileo_ephemeris.cc b/src/core/system_parameters/galileo_ephemeris.cc index 0a7d1df06..870254f67 100644 --- a/src/core/system_parameters/galileo_ephemeris.cc +++ b/src/core/system_parameters/galileo_ephemeris.cc @@ -78,7 +78,7 @@ double Galileo_Ephemeris::sv_clock_relativistic_term(double transmitTime) // Sa // 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 - const double tk = transmitTime - t0e_1; + const double tk = check_t(transmitTime - static_cast(t0e_1)); // Corrected mean motion 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; // 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 double E = M; @@ -99,7 +99,7 @@ double Galileo_Ephemeris::sv_clock_relativistic_term(double transmitTime) // Sa { E_old = 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) { // 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)); // Time from ephemeris reference epoch - const double tk = transmitTime - t0e_1; + const double tk = check_t(transmitTime - static_cast(t0e_1)); // Corrected mean motion const double n = n0 + delta_n_3; @@ -136,7 +136,7 @@ void Galileo_Ephemeris::satellitePosition(double transmitTime) double M = M0_1 + n * tk; // 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 double E = M; @@ -148,7 +148,7 @@ void Galileo_Ephemeris::satellitePosition(double transmitTime) { E_old = 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) { // 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 - const double tmp_Y = sqrt(1.0 - e_1 * e_1) * sin(E); - const double tmp_X = cos(E) - e_1; + const double tmp_Y = sq1e2 * sek; + const double tmp_X = cek - e_1; const double nu = atan2(tmp_Y, tmp_X); // Compute angle phi (argument of Latitude) double phi = nu + omega_2; // 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 - 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 - 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 - 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 - 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(t0e_1); // 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 - d_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega); - 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 - d_satpos_Z = sin(u) * r * sin(i); + const double xprime = r * cuk; + const double yprime = r * suk; + 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 - const double Omega_dot = OMEGA_dot_3 - 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); - d_satvel_Z = d_satpos_Y * sin(i); + const double xpkdot = rkdot * cuk - yprime * ukdot; + 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; +} + + +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; } diff --git a/src/core/system_parameters/galileo_ephemeris.h b/src/core/system_parameters/galileo_ephemeris.h index 4ca7b9b57..b2578ed7b 100644 --- a/src/core/system_parameters/galileo_ephemeris.h +++ b/src/core/system_parameters/galileo_ephemeris.h @@ -162,6 +162,14 @@ public: 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); }; diff --git a/src/core/system_parameters/gps_cnav_ephemeris.cc b/src/core/system_parameters/gps_cnav_ephemeris.cc index 21aad88d0..a8766b3e0 100644 --- a/src/core/system_parameters/gps_cnav_ephemeris.cc +++ b/src/core/system_parameters/gps_cnav_ephemeris.cc @@ -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 - const double tmp_Y = sqrt(1.0 - d_e_eccentricity * d_e_eccentricity) * sin(E); - const double tmp_X = cos(E) - d_e_eccentricity; + const double sq1e2 = sqrt(1.0 - d_e_eccentricity * 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); // Compute angle phi (argument of Latitude) const double phi = nu + d_OMEGA; + double pkdot = sq1e2 * ekdot / OneMinusecosE; // 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 - 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 - 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 - 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 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 sok = sin(Omega); + const double cok = cos(Omega); - // Reduce to between 0 and 2*pi rad - // Omega = fmod((Omega + 2*GNSS_PI), (2*GNSS_PI)); - - // --- Compute satellite coordinates in Earth-fixed coordinates - d_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega); - d_satpos_Y = cos(u) * r * sin(Omega) + sin(u) * r * cos(i) * cos(Omega); - d_satpos_Z = sin(u) * r * sin(i); + // Compute satellite coordinates in Earth-fixed coordinates + const double xprime = r * cuk; + const double yprime = r * suk; + 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 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); - d_satvel_Z = d_satpos_Y * sin(i); + + const double xpkdot = rkdot * cuk - yprime * ukdot; + 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 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; /* 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; } diff --git a/src/core/system_parameters/gps_ephemeris.cc b/src/core/system_parameters/gps_ephemeris.cc index ae804673f..73d432f0d 100644 --- a/src/core/system_parameters/gps_ephemeris.cc +++ b/src/core/system_parameters/gps_ephemeris.cc @@ -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 - const double tmp_Y = sqrt(1.0 - d_e_eccentricity * d_e_eccentricity) * sin(E); - const double tmp_X = cos(E) - d_e_eccentricity; + const double sq1e2 = sqrt(1.0 - d_e_eccentricity * 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); // Compute angle phi (argument of Latitude) const double phi = nu + d_OMEGA; + double pkdot = sq1e2 * ekdot / OneMinusecosE; // Reduce phi to between 0 and 2*pi rad // 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 - 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 - 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 - 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 const double Omega = d_OMEGA0 + (d_OMEGA_DOT - GNSS_OMEGA_EARTH_DOT) * tk - GNSS_OMEGA_EARTH_DOT * d_Toe; - - // Reduce to between 0 and 2*pi rad - // 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 - d_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega); - d_satpos_Y = cos(u) * r * sin(Omega) + sin(u) * r * cos(i) * cos(Omega); - d_satpos_Z = sin(u) * r * sin(i); + const double xprime = r * cuk; + const double yprime = r * suk; + 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 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); - d_satvel_Z = d_satpos_Y * sin(i); + + const double xpkdot = rkdot * cuk - yprime * ukdot; + 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 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; /* 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; }