diff --git a/src/core/system_parameters/gnss_ephemeris.cc b/src/core/system_parameters/gnss_ephemeris.cc index 101fbf63c..6e33f231b 100644 --- a/src/core/system_parameters/gnss_ephemeris.cc +++ b/src/core/system_parameters/gnss_ephemeris.cc @@ -17,10 +17,156 @@ #include "gnss_ephemeris.h" #include "MATH_CONSTANTS.h" +#include "gnss_frequencies.h" +#include #include +#include +#include +#include + + +double Gnss_Ephemeris::sv_clock_drift(double transmitTime) +{ + const double dt = check_t(transmitTime - this->toc); + this->dtr = sv_clock_relativistic_term(transmitTime); + this->satClkDrift = this->af0 + this->af1 * dt + this->af2 * (dt * dt) + this->dtr; + return this->satClkDrift; +} + + +double Gnss_Ephemeris::predicted_doppler(double rx_time_s, + double lat, + double lon, + double h, + double ve, + double vn, + double vu, + int band) const +{ + const double RE_WGS84 = 6378137.0; //!< earth semimajor axis (WGS84) (m) + const double FE_WGS84 = (1.0 / 298.257223563); //!< earth flattening (WGS84) + const double lat_rad = lat * D2R; + const double lon_rad = lon * D2R; + + const double sinp = sin(lat_rad); + const double cosp = cos(lat_rad); + const double sinl = sin(lon_rad); + const double cosl = cos(lon_rad); + + const double e2 = FE_WGS84 * (2.0 - FE_WGS84); + const double v = RE_WGS84 / std::sqrt(1.0 - e2 * sinp * sinp); + + // Position in EFEF + const std::vector pos_rx = {(v + h) * cosp * cosl, (v + h) * cosp * sinl, (v * (1.0 - e2) + h) * sinp}; + + // Velovity in EFEF + const double t = cosp * vu - sinp * vn; + const std::vector vel_rx = {cosl * t - sinl * ve, sinl * t + cosl * ve, sinp * vu + cosp * vn}; + + std::array sat_pos_vel = {0}; + satellitePosVelComputation(rx_time_s, sat_pos_vel); + const std::vector pos_sat = {sat_pos_vel[0], sat_pos_vel[1], sat_pos_vel[2]}; + const std::vector vel_sat = {sat_pos_vel[3], sat_pos_vel[4], sat_pos_vel[5]}; + + std::vector x_sr = pos_sat; + std::transform(x_sr.begin(), x_sr.end(), pos_rx.begin(), x_sr.begin(), std::minus()); // pos_sat - pos_rx + + const double norm_x_sr = std::sqrt(std::inner_product(x_sr.begin(), x_sr.end(), x_sr.begin(), 0.0)); // Euclidean norm + + std::vector v_sr = vel_sat; + std::transform(v_sr.begin(), v_sr.end(), vel_rx.begin(), v_sr.begin(), std::minus()); // vel_sat - vel_rx + + const double radial_vel = std::inner_product(v_sr.begin(), v_sr.end(), x_sr.begin(), 0.0) / norm_x_sr; + const double predicted_doppler_normalized = -(radial_vel / SPEED_OF_LIGHT_M_S); + double predicted_doppler = 0.0; + if (this->System == 'E') // Galileo + { + if (band == 1) + { + predicted_doppler = predicted_doppler_normalized * FREQ1; + } + else if (band == 5) + { + predicted_doppler = predicted_doppler_normalized * FREQ5; + } + else if (band == 6) + { + predicted_doppler = predicted_doppler_normalized * FREQ6; + } + else if (band == 7) + { + predicted_doppler = predicted_doppler_normalized * FREQ7; + } + else if (band == 8) + { + predicted_doppler = predicted_doppler_normalized * FREQ8; + } + else + { + predicted_doppler = 0.0; + } + } + else if (this->System == 'G') // GPS + { + if (band == 1) + { + predicted_doppler = predicted_doppler_normalized * FREQ1; + } + else if (band == 2) + { + predicted_doppler = predicted_doppler_normalized * FREQ2; + } + else if (band == 5) + { + predicted_doppler = predicted_doppler_normalized * FREQ5; + } + else + { + predicted_doppler = 0.0; + } + } + else if (this->System == 'B') // Beidou + { + if (band == 1) + { + predicted_doppler = predicted_doppler_normalized * FREQ1_BDS; + } + else if (band == 2) + { + predicted_doppler = predicted_doppler_normalized * FREQ2_BDS; + } + else if (band == 3) + { + predicted_doppler = predicted_doppler_normalized * FREQ3_BDS; + } + else + { + predicted_doppler = 0.0; + } + } + else + { + predicted_doppler = 0.0; + } + return predicted_doppler; +} void Gnss_Ephemeris::satellitePosition(double transmitTime) +{ + std::array pos_vel_dtr = {0}; + satellitePosVelComputation(transmitTime, pos_vel_dtr); + this->satpos_X = pos_vel_dtr[0]; + this->satpos_Y = pos_vel_dtr[1]; + this->satpos_Z = pos_vel_dtr[2]; + this->satvel_X = pos_vel_dtr[3]; + this->satvel_Y = pos_vel_dtr[4]; + this->satvel_Z = pos_vel_dtr[5]; + this->dtr = pos_vel_dtr[6]; +} + + +void Gnss_Ephemeris::satellitePosVelComputation(double transmitTime, std::array& pos_vel_dtr) const { // Restore semi-major axis const double a = this->sqrtA * this->sqrtA; @@ -128,45 +274,53 @@ void Gnss_Ephemeris::satellitePosition(double transmitTime) // --- Compute satellite coordinates in Earth-fixed coordinates const double xprime = r * cuk; const double yprime = r * suk; - this->satpos_X = xprime * cok - yprime * cik * sok; - this->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 - this->satpos_Z = yprime * sik; + + pos_vel_dtr[0] = xprime * cok - yprime * cik * sok; + pos_vel_dtr[1] = 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 + pos_vel_dtr[2] = yprime * sik; // Satellite's velocity. Can be useful for Vector Tracking loops const double xpkdot = rkdot * cuk - yprime * ukdot; const double ypkdot = rkdot * suk + xprime * ukdot; - const double tmp = ypkdot * cik - this->satpos_Z * ikdot; + const double tmp = ypkdot * cik - pos_vel_dtr[2] * ikdot; - this->satvel_X = -Omega_dot * this->satpos_Y + xpkdot * cok - tmp * sok; - this->satvel_Y = Omega_dot * this->satpos_X + xpkdot * sok + tmp * cok; - this->satvel_Z = yprime * cik * ikdot + ypkdot * sik; + pos_vel_dtr[3] = -Omega_dot * pos_vel_dtr[1] + xpkdot * cok - tmp * sok; + pos_vel_dtr[4] = Omega_dot * pos_vel_dtr[0] + xpkdot * sok + tmp * cok; + pos_vel_dtr[5] = yprime * cik * ikdot + ypkdot * sik; // Time from ephemeris reference clock tk = check_t(transmitTime - this->toc); - this->dtr = this->af0 + this->af1 * tk + this->af2 * tk * tk; + pos_vel_dtr[6] = this->af0 + this->af1 * tk + this->af2 * tk * tk; if (this->System == 'E') { - this->dtr -= 2.0 * sqrt(GALILEO_GM * a) * this->ecc * sek / (SPEED_OF_LIGHT_M_S * SPEED_OF_LIGHT_M_S); + pos_vel_dtr[6] -= 2.0 * sqrt(GALILEO_GM * a) * this->ecc * sek / (SPEED_OF_LIGHT_M_S * SPEED_OF_LIGHT_M_S); } else if (this->System == 'B') { - this->dtr -= 2.0 * sqrt(BEIDOU_GM * a) * this->ecc * sek / (SPEED_OF_LIGHT_M_S * SPEED_OF_LIGHT_M_S); + pos_vel_dtr[6] -= 2.0 * sqrt(BEIDOU_GM * a) * this->ecc * sek / (SPEED_OF_LIGHT_M_S * SPEED_OF_LIGHT_M_S); } else { - this->dtr -= 2.0 * sqrt(GPS_GM * a) * this->ecc * sek / (SPEED_OF_LIGHT_M_S * SPEED_OF_LIGHT_M_S); + pos_vel_dtr[6] -= 2.0 * sqrt(GPS_GM * a) * this->ecc * sek / (SPEED_OF_LIGHT_M_S * SPEED_OF_LIGHT_M_S); } } -double Gnss_Ephemeris::sv_clock_drift(double transmitTime) +double Gnss_Ephemeris::check_t(double time) const { - const double dt = check_t(transmitTime - this->toc); - this->dtr = sv_clock_relativistic_term(transmitTime); - this->satClkDrift = this->af0 + this->af1 * dt + this->af2 * (dt * dt) + this->dtr; - return this->satClkDrift; + 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; } @@ -184,7 +338,7 @@ double Gnss_Ephemeris::sv_clock_relativistic_term(double transmitTime) const { n0 = sqrt(GALILEO_GM / (a * a * a)); } - else if (this->System == 'E') + else if (this->System == 'B') { n0 = sqrt(BEIDOU_GM / (a * a * a)); } @@ -235,19 +389,3 @@ double Gnss_Ephemeris::sv_clock_relativistic_term(double transmitTime) const } return dtr_; } - - -double Gnss_Ephemeris::check_t(double time) const -{ - 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/gnss_ephemeris.h b/src/core/system_parameters/gnss_ephemeris.h index 644f430d4..a5549bff6 100644 --- a/src/core/system_parameters/gnss_ephemeris.h +++ b/src/core/system_parameters/gnss_ephemeris.h @@ -19,6 +19,7 @@ #ifndef GNSS_SDR_GNSS_EPHEMERIS_H #define GNSS_SDR_GNSS_EPHEMERIS_H +#include #include /*! @@ -36,6 +37,20 @@ public: */ double sv_clock_drift(double transmitTime); + /*! + * \brief Computes prediction of the Doppler shift for a given time and receiver's position and velocity. + * @param[in] rx_time_s Time of Week in seconds + * @param[in] lat Receiver's latitude in degrees + * @param[in] lon Receiver's longitude in degrees + * @param[in] h Receiver's height in meters + * @param[in] ve Receiver's velocity in the East direction [m/s] + * @param[in] vn Receiver's velocity in the North direction [m/s] + * @param[in] vu Receiver's velocity in the Up direction [m/s] + * @param[in] band Signal band for which the Doppler will be computed + * (1: L1 C/A, E1B, BI1; 2: L2C, BI2; 3: BI3; 5: L5/E5a; 6: E6B; 7: E5b; 8: E5a+E5b) + */ + double predicted_doppler(double rx_time_s, double lat, double lon, double h, double ve, double vn, double vu, int band) const; + void satellitePosition(double transmitTime); //!< Computes the ECEF SV coordinates and ECEF velocity uint32_t PRN{}; //!< SV ID @@ -83,6 +98,7 @@ protected: char System{}; //!< Character ID of the GNSS system. 'G': GPS. 'E': Galileo. 'B': BeiDou private: + void satellitePosVelComputation(double transmitTime, std::array& pos_vel_dtr) const; double check_t(double time) const; double sv_clock_relativistic_term(double transmitTime) const; };