| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // This file is part of INSTINCT, the INS Toolkit for Integrated | ||
| 2 | // Navigation Concepts and Training by the Institute of Navigation of | ||
| 3 | // the University of Stuttgart, Germany. | ||
| 4 | // | ||
| 5 | // This Source Code Form is subject to the terms of the Mozilla Public | ||
| 6 | // License, v. 2.0. If a copy of the MPL was not distributed with this | ||
| 7 | // file, You can obtain one at https://mozilla.org/MPL/2.0/. | ||
| 8 | |||
| 9 | #include "GPSEphemeris.hpp" | ||
| 10 | |||
| 11 | #include "Navigation/Constants.hpp" | ||
| 12 | #include "Navigation/GNSS/Functions.hpp" | ||
| 13 | |||
| 14 | #include "util/Logger.hpp" | ||
| 15 | |||
| 16 | namespace NAV | ||
| 17 | { | ||
| 18 | |||
| 19 | 19 | GPSEphemeris::GPSEphemeris(const InsTime& toc) | |
| 20 | 19 | : SatNavData(SatNavData::GPSEphemeris, toc) {} | |
| 21 | |||
| 22 | 1204 | GPSEphemeris::GPSEphemeris(const InsTime& toc, const InsTime& toe, | |
| 23 | const size_t& IODE, const size_t& IODC, | ||
| 24 | const std::array<double, 3>& a, | ||
| 25 | const double& sqrt_A, const double& e, const double& i_0, const double& Omega_0, const double& omega, const double& M_0, | ||
| 26 | const double& delta_n, const double& Omega_dot, const double& i_dot, const double& Cus, const double& Cuc, | ||
| 27 | const double& Cis, const double& Cic, const double& Crs, const double& Crc, | ||
| 28 | const double& svAccuracy, uint8_t svHealth, | ||
| 29 | uint8_t L2ChannelCodes, bool L2DataFlagPCode, | ||
| 30 | const double& T_GD, | ||
| 31 | 1204 | const double& fitInterval) | |
| 32 | : SatNavData(SatNavData::GPSEphemeris, toc), | ||
| 33 | 1204 | toc(toc), | |
| 34 | 1204 | toe(toe), | |
| 35 | 1204 | IODE(IODE), | |
| 36 | 1204 | IODC(IODC), | |
| 37 | 1204 | a(a), | |
| 38 | 1204 | sqrt_A(sqrt_A), | |
| 39 | 1204 | e(e), | |
| 40 | 1204 | i_0(i_0), | |
| 41 | 1204 | Omega_0(Omega_0), | |
| 42 | 1204 | omega(omega), | |
| 43 | 1204 | M_0(M_0), | |
| 44 | 1204 | delta_n(delta_n), | |
| 45 | 1204 | Omega_dot(Omega_dot), | |
| 46 | 1204 | i_dot(i_dot), | |
| 47 | 1204 | Cus(Cus), | |
| 48 | 1204 | Cuc(Cuc), | |
| 49 | 1204 | Cis(Cis), | |
| 50 | 1204 | Cic(Cic), | |
| 51 | 1204 | Crs(Crs), | |
| 52 | 1204 | Crc(Crc), | |
| 53 | 1204 | svAccuracy(svAccuracy), | |
| 54 | 1204 | svHealth(svHealth), | |
| 55 | 1204 | L2ChannelCodes(L2ChannelCodes), | |
| 56 | 1204 | L2DataFlagPCode(L2DataFlagPCode), | |
| 57 | 1204 | T_GD(T_GD), | |
| 58 | 1204 | fitInterval(fitInterval) {} | |
| 59 | |||
| 60 | #ifdef TESTING | ||
| 61 | |||
| 62 | 8941 | GPSEphemeris::GPSEphemeris(int32_t year, int32_t month, int32_t day, int32_t hour, int32_t minute, double second, double svClockBias, double svClockDrift, double svClockDriftRate, | |
| 63 | double IODE, double Crs, double delta_n, double M_0, | ||
| 64 | double Cuc, double e, double Cus, double sqrt_A, | ||
| 65 | double Toe, double Cic, double Omega_0, double Cis, | ||
| 66 | double i_0, double Crc, double omega, double Omega_dot, | ||
| 67 | double i_dot, double L2ChannelCodes, double GPSWeek, double L2DataFlagPCode, | ||
| 68 | double svAccuracy, double svHealth, double T_GD, double IODC, | ||
| 69 | 8941 | double /* TransmissionTimeOfMessage */, double fitInterval, double /* spare1 */, double /* spare2 */) | |
| 70 |
2/4✓ Branch 1 taken 8941 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8941 times.
✗ Branch 5 not taken.
|
8941 | : SatNavData(SatNavData::GPSEphemeris, InsTime(year, month, day, hour, minute, second, SatelliteSystem(GPS).getTimeSystem())), |
| 71 | 8941 | toc(refTime), | |
| 72 |
2/4✓ Branch 1 taken 8941 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8941 times.
✗ Branch 5 not taken.
|
8941 | toe(InsTime(0, static_cast<int32_t>(GPSWeek), Toe, SatelliteSystem(GPS).getTimeSystem())), |
| 73 | 8941 | IODE(static_cast<size_t>(IODE)), | |
| 74 | 8941 | IODC(static_cast<size_t>(IODC)), | |
| 75 | 8941 | a({ svClockBias, svClockDrift, svClockDriftRate }), | |
| 76 | 8941 | sqrt_A(sqrt_A), | |
| 77 | 8941 | e(e), | |
| 78 | 8941 | i_0(i_0), | |
| 79 | 8941 | Omega_0(Omega_0), | |
| 80 | 8941 | omega(omega), | |
| 81 | 8941 | M_0(M_0), | |
| 82 | 8941 | delta_n(delta_n), | |
| 83 | 8941 | Omega_dot(Omega_dot), | |
| 84 | 8941 | i_dot(i_dot), | |
| 85 | 8941 | Cus(Cus), | |
| 86 | 8941 | Cuc(Cuc), | |
| 87 | 8941 | Cis(Cis), | |
| 88 | 8941 | Cic(Cic), | |
| 89 | 8941 | Crs(Crs), | |
| 90 | 8941 | Crc(Crc), | |
| 91 | 8941 | svAccuracy(svAccuracy), | |
| 92 | 8941 | svHealth(static_cast<uint8_t>(svHealth)), | |
| 93 | 8941 | L2ChannelCodes(static_cast<uint8_t>(L2ChannelCodes)), | |
| 94 | 8941 | L2DataFlagPCode(static_cast<bool>(L2DataFlagPCode)), | |
| 95 | 8941 | T_GD(T_GD), | |
| 96 |
1/2✓ Branch 2 taken 8941 times.
✗ Branch 3 not taken.
|
17882 | fitInterval(fitInterval) |
| 97 | 8941 | {} | |
| 98 | |||
| 99 | #endif | ||
| 100 | |||
| 101 | 175975 | Clock::Corrections GPSEphemeris::calcClockCorrections(const InsTime& recvTime, double dist, const Frequency& freq) const | |
| 102 | { | ||
| 103 | LOG_DATA("Calc Sat Clock corrections at receiver time {}", recvTime.toGPSweekTow()); | ||
| 104 | // Earth gravitational constant [m³/s²] (WGS 84 value of the earth's gravitational constant for GPS user) | ||
| 105 | 175975 | const auto mu = InsConst::GPS::MU; | |
| 106 | // Relativistic constant F for clock corrections [s/√m] (-2*√µ/c²) | ||
| 107 | 175975 | const auto F = InsConst::GPS::F; | |
| 108 | |||
| 109 | LOG_DATA(" toe {} [{}] (Time of ephemeris)", toe.toGPSweekTow(), toe.toYMDHMS(GPST)); | ||
| 110 | |||
| 111 | 175975 | const auto A = sqrt_A * sqrt_A; // Semi-major axis [m] | |
| 112 | LOG_DATA(" A {} [m] (Semi-major axis)", A); | ||
| 113 | 175975 | auto n_0 = std::sqrt(mu / std::pow(A, 3)); // Computed mean motion [rad/s] | |
| 114 | LOG_DATA(" n_0 {} [rad/s] (Computed mean motion)", n_0); | ||
| 115 | 175975 | auto n = n_0 + delta_n; // Corrected mean motion [rad/s] | |
| 116 | LOG_DATA(" n {} [rad/s] (Corrected mean motion)", n); | ||
| 117 | |||
| 118 | // Time at transmission | ||
| 119 |
2/4✓ Branch 2 taken 175975 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 175975 times.
✗ Branch 6 not taken.
|
175975 | InsTime transTime0 = recvTime - std::chrono::duration<double>(dist / InsConst::C); |
| 120 | |||
| 121 | 175975 | InsTime transTime = transTime0; | |
| 122 | LOG_DATA(" Iterating Time at transmission"); | ||
| 123 | 175975 | double dt_sv = 0.0; | |
| 124 | 175975 | double clkDrift = 0.0; | |
| 125 | |||
| 126 |
2/2✓ Branch 0 taken 351944 times.
✓ Branch 1 taken 175978 times.
|
527922 | for (size_t i = 0; i < 2; i++) |
| 127 | { | ||
| 128 | LOG_DATA(" [{}]", i); | ||
| 129 | LOG_DATA(" transTime {} [{}] (Time at transmission)", transTime.toGPSweekTow(), transTime.toYMDHMS(GPST)); | ||
| 130 | |||
| 131 | // [s] | ||
| 132 |
1/2✓ Branch 1 taken 351946 times.
✗ Branch 2 not taken.
|
351944 | auto t_minus_toc = static_cast<double>((transTime - toc).count()); |
| 133 | LOG_DATA(" transTime - toc {} [s]", t_minus_toc); | ||
| 134 | |||
| 135 | // Time difference from ephemeris reference epoch [s] | ||
| 136 |
1/2✓ Branch 1 taken 351947 times.
✗ Branch 2 not taken.
|
351946 | double t_k = static_cast<double>((transTime - toe).count()); |
| 137 | LOG_DATA(" transTime - toe {} [s] (t_k = Time difference from ephemeris reference epoch)", t_k); | ||
| 138 | |||
| 139 | // Mean anomaly [rad] | ||
| 140 | 351948 | auto M_k = M_0 + n * t_k; | |
| 141 | LOG_DATA(" M_k {} [s] (Mean anomaly)", M_k); | ||
| 142 | |||
| 143 | // Eccentric anomaly [rad] | ||
| 144 | 351948 | double E_k = M_k; | |
| 145 | 351948 | double E_k_old = 0.0; | |
| 146 | |||
| 147 |
5/6✓ Branch 1 taken 1979103 times.
✓ Branch 2 taken 351938 times.
✓ Branch 3 taken 1979103 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 1979093 times.
✓ Branch 6 taken 351948 times.
|
2331041 | for (size_t i = 0; std::abs(E_k - E_k_old) > 1e-13 && i < 10; i++) |
| 148 | { | ||
| 149 | 1979093 | E_k_old = E_k; // Kepler’s equation ( Mk = E_k − e sin E_k ) may be solved for Eccentric anomaly (E_k) by iteration: | |
| 150 | 1979093 | E_k = M_k + e * sin(E_k); | |
| 151 | } | ||
| 152 | |||
| 153 | // Relativistic correction term [s] | ||
| 154 | 351948 | double dt_r = F * e * sqrt_A * std::sin(E_k); | |
| 155 | LOG_DATA(" dt_r {} [s] (Relativistic correction term)", dt_r); | ||
| 156 | |||
| 157 | // SV PRN code phase time offset [s] | ||
| 158 | 351948 | dt_sv = a[0] + a[1] * t_minus_toc + a[2] * std::pow(t_minus_toc, 2) + dt_r; | |
| 159 | |||
| 160 | // See IS-GPS-200M GPS ICD L1/L2, ch. 20.3.3.3.3.2, p.99 | ||
| 161 | // See IS-GPS-705J GPS ICD L5, ch. 20.3.3.3.2.1, p.78 | ||
| 162 |
3/4✓ Branch 2 taken 235722 times.
✓ Branch 3 taken 116228 times.
✓ Branch 6 taken 116228 times.
✗ Branch 7 not taken.
|
351947 | dt_sv -= freq & (G01 | G05) ? T_GD : ratioFreqSquared(G01, freq, -128, -128) * T_GD; |
| 163 | |||
| 164 | LOG_DATA(" dt_sv {} [s] (SV PRN code phase time offset)", dt_sv); | ||
| 165 | |||
| 166 | // Groves ch. 9.3.1, eq. 9.78, p. 391 | ||
| 167 | 351950 | clkDrift = a[1] + a[2] / 2.0 * t_minus_toc; | |
| 168 | |||
| 169 | // Correct transmit time for the satellite clock bias | ||
| 170 |
2/4✓ Branch 2 taken 351946 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 351947 times.
✗ Branch 6 not taken.
|
351946 | transTime = transTime0 - std::chrono::duration<double>(dt_sv); |
| 171 | } | ||
| 172 | LOG_DATA(" transTime {} [{}] (Time at transmission)", transTime.toGPSweekTow(), transTime.toYMDHMS(GPST)); | ||
| 173 | |||
| 174 | 175978 | return { .transmitTime = transTime, .bias = dt_sv, .drift = clkDrift }; | |
| 175 | } | ||
| 176 | |||
| 177 | 181555 | Orbit::PosVelAccel GPSEphemeris::calcSatelliteData(const InsTime& transTime, Orbit::Calc calc) const | |
| 178 | { | ||
| 179 |
2/4✓ Branch 1 taken 181555 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 181555 times.
✗ Branch 5 not taken.
|
181555 | Eigen::Vector3d e_pos = Eigen::Vector3d::Zero(); |
| 180 |
2/4✓ Branch 1 taken 181555 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 181554 times.
✗ Branch 5 not taken.
|
181555 | Eigen::Vector3d e_vel = Eigen::Vector3d::Zero(); |
| 181 |
2/4✓ Branch 1 taken 181555 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 181554 times.
✗ Branch 5 not taken.
|
181554 | Eigen::Vector3d e_accel = Eigen::Vector3d::Zero(); |
| 182 | |||
| 183 | LOG_DATA("Calc Sat Position at transmit time {}", transTime.toGPSweekTow()); | ||
| 184 | // Earth gravitational constant [m³/s²] (WGS 84 value of the earth's gravitational constant for GPS user) | ||
| 185 | 181554 | const auto mu = InsConst::GPS::MU; | |
| 186 | // Earth angular velocity [rad/s] (WGS 84 value of the earth's rotation rate) | ||
| 187 | 181554 | const auto Omega_e_dot = InsConst::GPS::omega_ie; | |
| 188 | |||
| 189 | LOG_DATA(" toe {} (Time of ephemeris)", toe.toGPSweekTow()); | ||
| 190 | |||
| 191 | 181554 | const auto A = sqrt_A * sqrt_A; // Semi-major axis [m] | |
| 192 | LOG_DATA(" A {} [m] (Semi-major axis)", A); | ||
| 193 | 181554 | auto n_0 = std::sqrt(mu / std::pow(A, 3)); // Computed mean motion [rad/s] | |
| 194 | LOG_DATA(" n_0 {} [rad/s] (Computed mean motion)", n_0); | ||
| 195 | 181554 | auto n = n_0 + delta_n; // Corrected mean motion [rad/s] | |
| 196 | LOG_DATA(" n {} [rad/s] (Corrected mean motion)", n); | ||
| 197 | |||
| 198 | // Eccentric anomaly [rad] | ||
| 199 | 181554 | double E_k = 0.0; | |
| 200 | |||
| 201 | // Time difference from ephemeris reference epoch [s] | ||
| 202 |
1/2✓ Branch 1 taken 181555 times.
✗ Branch 2 not taken.
|
181554 | double t_k = static_cast<double>((transTime - toe).count()); |
| 203 | LOG_DATA(" t_k {} [s] (Time difference from ephemeris reference epoch)", t_k); | ||
| 204 | |||
| 205 | // Mean anomaly [rad] | ||
| 206 | 181555 | auto M_k = M_0 + n * t_k; | |
| 207 | LOG_DATA(" M_k {} [s] (Mean anomaly)", M_k); | ||
| 208 | |||
| 209 | 181555 | E_k = M_k; // Initial Value [rad] | |
| 210 | 181555 | double E_k_old = 0.0; | |
| 211 | LOG_DATA(" Iterating E_k"); | ||
| 212 | LOG_DATA(" E_k {} [rad] (Eccentric anomaly)", E_k); | ||
| 213 |
5/6✓ Branch 1 taken 544672 times.
✓ Branch 2 taken 181555 times.
✓ Branch 3 taken 544672 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 544672 times.
✓ Branch 6 taken 181555 times.
|
726227 | for (size_t i = 0; std::abs(E_k - E_k_old) > 1e-13 && i < 10; i++) |
| 214 | { | ||
| 215 | 544672 | E_k_old = E_k; // Kepler’s equation ( Mk = E_k − e sin E_k ) may be solved for Eccentric anomaly (E_k) by iteration: | |
| 216 | 544672 | E_k = E_k + (M_k - E_k + e * std::sin(E_k)) / (1 - e * std::cos(E_k)); // - Refined Value, minimum of three iterations, (j=1,2,3) | |
| 217 | LOG_DATA(" E_k {} [rad] (Eccentric anomaly)", E_k); // - Final Value (radians) | ||
| 218 | } | ||
| 219 | |||
| 220 | // auto v_k = 2.0 * std::atan(std::sqrt((1.0 + e) / (1.0 - e)) * std::tan(E_k / 2.0)); // True Anomaly (unambiguous quadrant) [rad] (GPS ICD algorithm) | ||
| 221 | // auto v_k = std::atan2(std::sqrt(1 - e * e) * std::sin(E_k) / (1 - e * std::cos(E_k)), (std::cos(E_k) - e) / (1 - e * std::cos(E_k))); // True Anomaly [rad] (GALILEO ICD algorithm) | ||
| 222 | 181555 | auto v_k = std::atan2(std::sqrt(1 - e * e) * std::sin(E_k), (std::cos(E_k) - e)); // True Anomaly [rad] // simplified, since the denominators cancel out | |
| 223 | LOG_DATA(" v_k {} [rad] (True Anomaly (unambiguous quadrant))", v_k); | ||
| 224 | 181555 | auto Phi_k = v_k + omega; // Argument of Latitude [rad] | |
| 225 | LOG_DATA(" Phi_k {} [rad] (Argument of Latitude)", Phi_k); | ||
| 226 | |||
| 227 | // Second Harmonic Perturbations | ||
| 228 | 181555 | auto delta_u_k = Cus * std::sin(2 * Phi_k) + Cuc * std::cos(2 * Phi_k); // Argument of Latitude Correction [rad] | |
| 229 | LOG_DATA(" delta_u_k {} [rad] (Argument of Latitude Correction)", delta_u_k); | ||
| 230 | 181555 | auto delta_r_k = Crs * std::sin(2 * Phi_k) + Crc * std::cos(2 * Phi_k); // Radius Correction [m] | |
| 231 | LOG_DATA(" delta_r_k {} [m] (Radius Correction)", delta_r_k); | ||
| 232 | 181555 | auto delta_i_k = Cis * std::sin(2 * Phi_k) + Cic * std::cos(2 * Phi_k); // Inclination Correction [rad] | |
| 233 | LOG_DATA(" delta_i_k {} [rad] (Inclination Correction)", delta_i_k); | ||
| 234 | |||
| 235 | 181555 | auto u_k = Phi_k + delta_u_k; // Corrected Argument of Latitude [rad] | |
| 236 | LOG_DATA(" u_k {} [rad] (Corrected Argument of Latitude)", u_k); | ||
| 237 | 181555 | auto r_k = A * (1 - e * std::cos(E_k)) + delta_r_k; // Corrected Radius [m] | |
| 238 | LOG_DATA(" r_k {} [m] (Corrected Radius)", r_k); | ||
| 239 | 181555 | auto i_k = i_0 + delta_i_k + i_dot * t_k; // Corrected Inclination [rad] | |
| 240 | LOG_DATA(" i_k {} [rad] (Corrected Inclination)", i_k); | ||
| 241 | |||
| 242 | 181555 | auto x_k_op = r_k * std::cos(u_k); // Position in orbital plane [m] | |
| 243 | LOG_DATA(" x_k_op {} [m] (Position in orbital plane)", x_k_op); | ||
| 244 | 181555 | auto y_k_op = r_k * std::sin(u_k); // Position in orbital plane [m] | |
| 245 | LOG_DATA(" y_k_op {} [m] (Position in orbital plane)", y_k_op); | ||
| 246 | |||
| 247 | // Corrected longitude of ascending node [rad] | ||
| 248 |
1/2✓ Branch 2 taken 181554 times.
✗ Branch 3 not taken.
|
181555 | auto Omega_k = Omega_0 + (Omega_dot - Omega_e_dot) * t_k - Omega_e_dot * static_cast<double>(toe.toGPSweekTow().tow); |
| 249 | LOG_DATA(" Omega_k {} [rad] (Corrected longitude of ascending node)", Omega_k); | ||
| 250 | |||
| 251 | // Earth-fixed x coordinates [m] | ||
| 252 | 181554 | auto x_k = x_k_op * std::cos(Omega_k) - y_k_op * std::cos(i_k) * std::sin(Omega_k); | |
| 253 | LOG_DATA(" x_k {} [m] (Earth-fixed x coordinates)", x_k); | ||
| 254 | // Earth-fixed y coordinates [m] | ||
| 255 | 181554 | auto y_k = x_k_op * std::sin(Omega_k) + y_k_op * std::cos(i_k) * std::cos(Omega_k); | |
| 256 | LOG_DATA(" y_k {} [m] (Earth-fixed y coordinates)", y_k); | ||
| 257 | // Earth-fixed z coordinates [m] | ||
| 258 | 181554 | auto z_k = y_k_op * std::sin(i_k); | |
| 259 | LOG_DATA(" z_k {} [m] (Earth-fixed z coordinates)", z_k); | ||
| 260 | |||
| 261 |
1/2✓ Branch 1 taken 181555 times.
✗ Branch 2 not taken.
|
181554 | e_pos = Eigen::Vector3d{ x_k, y_k, z_k }; |
| 262 | |||
| 263 |
5/6✓ Branch 1 taken 7879 times.
✓ Branch 2 taken 173676 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 7879 times.
✓ Branch 6 taken 173676 times.
✓ Branch 7 taken 7879 times.
|
181555 | if (calc & Calc_Velocity || calc & Calc_Acceleration) |
| 264 | { | ||
| 265 | // Eccentric Anomaly Rate [rad/s] | ||
| 266 | 173676 | auto E_k_dot = n / (1 - e * std::cos(E_k)); | |
| 267 | // True Anomaly Rate [rad/s] | ||
| 268 | 173676 | auto v_k_dot = E_k_dot * std::sqrt(1 - e * e) / (1 - e * std::cos(E_k)); | |
| 269 | // Corrected Inclination Angle Rate [rad/s] | ||
| 270 | 173676 | auto i_k_dot = i_dot + 2 * v_k_dot * (Cis * std::cos(2 * Phi_k) - Cic * std::sin(2 * Phi_k)); | |
| 271 | // Corrected Argument of Latitude Rate [rad/s] | ||
| 272 | 173676 | auto u_k_dot = v_k_dot + 2 * v_k_dot * (Cus * std::cos(2 * Phi_k) - Cuc * std::sin(2 * Phi_k)); | |
| 273 | // Corrected Radius Rate [m/s] | ||
| 274 | 173676 | auto r_k_dot = e * A * E_k_dot * std::sin(E_k) + 2 * v_k_dot * (Crs * std::cos(2 * Phi_k) - Crc * std::sin(2 * Phi_k)); | |
| 275 | // Longitude of Ascending Node Rate [rad/s] | ||
| 276 | 173676 | auto Omega_k_dot = Omega_dot - Omega_e_dot; | |
| 277 | // In-plane x velocity [m/s] | ||
| 278 | 173676 | auto vx_k_op = r_k_dot * std::cos(u_k) - r_k * u_k_dot * std::sin(u_k); | |
| 279 | // In-plane y velocity [m/s] | ||
| 280 | 173676 | auto vy_k_op = r_k_dot * std::sin(u_k) + r_k * u_k_dot * std::cos(u_k); | |
| 281 | // Earth-Fixed x velocity [m/s] | ||
| 282 | 173676 | auto vx_k = -x_k_op * Omega_k_dot * std::sin(Omega_k) + vx_k_op * std::cos(Omega_k) - vy_k_op * std::sin(Omega_k) * std::cos(i_k) | |
| 283 | 173676 | - y_k_op * (Omega_k_dot * std::cos(Omega_k) * std::cos(i_k) - i_k_dot * std::sin(Omega_k) * std::sin(i_k)); | |
| 284 | // Earth-Fixed y velocity [m/s] | ||
| 285 | 173676 | auto vy_k = x_k_op * Omega_k_dot * std::cos(Omega_k) + vx_k_op * std::sin(Omega_k) + vy_k_op * std::cos(Omega_k) * std::cos(i_k) | |
| 286 | 173676 | - y_k_op * (Omega_k_dot * std::sin(Omega_k) * std::cos(i_k) + i_k_dot * std::cos(Omega_k) * std::sin(i_k)); | |
| 287 | // Earth-Fixed z velocity [m/s] | ||
| 288 | 173676 | auto vz_k = vy_k_op * std::sin(i_k) + y_k_op * i_k_dot * std::cos(i_k); | |
| 289 | |||
| 290 |
1/2✓ Branch 1 taken 173675 times.
✗ Branch 2 not taken.
|
173676 | if (calc & Calc_Velocity) |
| 291 | { | ||
| 292 |
1/2✓ Branch 1 taken 173674 times.
✗ Branch 2 not taken.
|
173675 | e_vel = Eigen::Vector3d{ vx_k, vy_k, vz_k }; |
| 293 | } | ||
| 294 | |||
| 295 |
2/2✓ Branch 1 taken 2790 times.
✓ Branch 2 taken 170884 times.
|
173674 | if (calc & Calc_Acceleration) |
| 296 | { | ||
| 297 | // Oblate Earth acceleration Factor [m/s^2] | ||
| 298 | 2790 | auto F = -(3.0 / 2.0) * InsConst::GPS::J2 * (mu / std::pow(r_k, 2)) * std::pow(InsConst::GPS::R_E / r_k, 2); | |
| 299 | // Earth-Fixed x acceleration [m/s^2] | ||
| 300 | 2790 | auto ax_k = -mu * (x_k / std::pow(r_k, 3)) + F * ((1.0 - 5.0 * std::pow(z_k / r_k, 2)) * (x_k / r_k)) | |
| 301 | 2790 | + 2 * vy_k * Omega_e_dot + x_k * std::pow(Omega_e_dot, 2); | |
| 302 | // Earth-Fixed y acceleration [m/s^2] | ||
| 303 | 2790 | auto ay_k = -mu * (y_k / std::pow(r_k, 3)) + F * ((1.0 - 5.0 * std::pow(z_k / r_k, 2)) * (y_k / r_k)) | |
| 304 | 2790 | + 2 * vx_k * Omega_e_dot + y_k * std::pow(Omega_e_dot, 2); | |
| 305 | // Earth-Fixed z acceleration [m/s^2] | ||
| 306 | 2790 | auto az_k = -mu * (z_k / std::pow(r_k, 3)) + F * ((3.0 - 5.0 * std::pow(z_k / r_k, 2)) * (z_k / r_k)); | |
| 307 | |||
| 308 |
1/2✓ Branch 1 taken 2790 times.
✗ Branch 2 not taken.
|
2790 | e_accel = Eigen::Vector3d{ ax_k, ay_k, az_k }; |
| 309 | } | ||
| 310 | } | ||
| 311 | |||
| 312 | return { .e_pos = e_pos, | ||
| 313 | .e_vel = e_vel, | ||
| 314 |
3/6✓ Branch 1 taken 181553 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 181553 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 181555 times.
✗ Branch 8 not taken.
|
363108 | .e_accel = e_accel }; |
| 315 | } | ||
| 316 | |||
| 317 | 175949 | bool GPSEphemeris::isHealthy() const | |
| 318 | { | ||
| 319 | 175949 | return svHealth == 0; | |
| 320 | } | ||
| 321 | |||
| 322 | 68587 | double GPSEphemeris::calcSatellitePositionVariance() const | |
| 323 | { | ||
| 324 | 68587 | return std::pow(svAccuracy, 2); | |
| 325 | } | ||
| 326 | |||
| 327 | } // namespace NAV | ||
| 328 |