| 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 "BDSEphemeris.hpp" | ||
| 10 | #include <chrono> | ||
| 11 | |||
| 12 | #include "Navigation/Constants.hpp" | ||
| 13 | #include "Navigation/GNSS/Core/SatelliteIdentifier.hpp" | ||
| 14 | #include "Navigation/GNSS/Functions.hpp" | ||
| 15 | |||
| 16 | #include "Navigation/Transformations/Units.hpp" | ||
| 17 | #include "util/Logger.hpp" | ||
| 18 | #include <Eigen/src/Core/Matrix.h> | ||
| 19 | |||
| 20 | namespace NAV | ||
| 21 | { | ||
| 22 | |||
| 23 | 160 | BDSEphemeris::BDSEphemeris(const uint16_t& satNum, const InsTime& toc, const InsTime& toe, | |
| 24 | const size_t& AODE, const size_t& AODC, | ||
| 25 | const std::array<double, 3>& a, | ||
| 26 | const double& sqrt_A, const double& e, const double& i_0, const double& Omega_0, const double& omega, const double& M_0, | ||
| 27 | const double& delta_n, const double& Omega_dot, const double& i_dot, const double& Cus, const double& Cuc, | ||
| 28 | const double& Cis, const double& Cic, const double& Crs, const double& Crc, | ||
| 29 | 160 | const double& svAccuracy, uint8_t satH1, double T_GD1, double T_GD2) | |
| 30 | : SatNavData(SatNavData::BeiDouEphemeris, toc), | ||
| 31 | 160 | satNum(satNum), | |
| 32 | 160 | toc(toc), | |
| 33 | 160 | toe(toe), | |
| 34 | 160 | AODE(AODE), | |
| 35 | 160 | AODC(AODC), | |
| 36 | 160 | a(a), | |
| 37 | 160 | sqrt_A(sqrt_A), | |
| 38 | 160 | e(e), | |
| 39 | 160 | i_0(i_0), | |
| 40 | 160 | Omega_0(Omega_0), | |
| 41 | 160 | omega(omega), | |
| 42 | 160 | M_0(M_0), | |
| 43 | 160 | delta_n(delta_n), | |
| 44 | 160 | Omega_dot(Omega_dot), | |
| 45 | 160 | i_dot(i_dot), | |
| 46 | 160 | Cus(Cus), | |
| 47 | 160 | Cuc(Cuc), | |
| 48 | 160 | Cis(Cis), | |
| 49 | 160 | Cic(Cic), | |
| 50 | 160 | Crs(Crs), | |
| 51 | 160 | Crc(Crc), | |
| 52 | 160 | svAccuracy(svAccuracy), | |
| 53 | 160 | satH1(satH1), | |
| 54 | 160 | T_GD1(T_GD1), | |
| 55 | 160 | T_GD2(T_GD2) | |
| 56 | 160 | {} | |
| 57 | |||
| 58 | #ifdef TESTING | ||
| 59 | |||
| 60 | 6730 | BDSEphemeris::BDSEphemeris(int32_t satNum, int32_t year, int32_t month, int32_t day, int32_t hour, int32_t minute, double second, double svClockBias, double svClockDrift, double svClockDriftRate, | |
| 61 | double AODE, double Crs, double delta_n, double M_0, | ||
| 62 | double Cuc, double e, double Cus, double sqrt_A, | ||
| 63 | double Toe, double Cic, double Omega_0, double Cis, | ||
| 64 | double i_0, double Crc, double omega, double Omega_dot, | ||
| 65 | double i_dot, double /* spare1 */, double BDTWeek, double /* spare2 */, | ||
| 66 | double svAccuracy, double satH1, double T_GD1, double T_GD2, | ||
| 67 | 6730 | double /* TransmissionTimeOfMessage */, double AODC, double /* spare3 */, double /* spare4 */) | |
| 68 |
2/4✓ Branch 1 taken 6730 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6730 times.
✗ Branch 5 not taken.
|
6730 | : SatNavData(SatNavData::BeiDouEphemeris, InsTime(year, month, day, hour, minute, second, SatelliteSystem(BDS).getTimeSystem())), |
| 69 | 6730 | satNum(static_cast<uint16_t>(satNum)), | |
| 70 | 6730 | toc(refTime), | |
| 71 |
2/4✓ Branch 1 taken 6730 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6730 times.
✗ Branch 5 not taken.
|
6730 | toe(InsTime(0, static_cast<int32_t>(BDTWeek) + InsTimeUtil::DIFF_BDT_WEEK_TO_GPST_WEEK, Toe, SatelliteSystem(BDS).getTimeSystem())), |
| 72 | 6730 | AODE(static_cast<size_t>(AODE)), | |
| 73 | 6730 | AODC(static_cast<size_t>(AODC)), | |
| 74 | 6730 | a({ svClockBias, svClockDrift, svClockDriftRate }), | |
| 75 | 6730 | sqrt_A(sqrt_A), | |
| 76 | 6730 | e(e), | |
| 77 | 6730 | i_0(i_0), | |
| 78 | 6730 | Omega_0(Omega_0), | |
| 79 | 6730 | omega(omega), | |
| 80 | 6730 | M_0(M_0), | |
| 81 | 6730 | delta_n(delta_n), | |
| 82 | 6730 | Omega_dot(Omega_dot), | |
| 83 | 6730 | i_dot(i_dot), | |
| 84 | 6730 | Cus(Cus), | |
| 85 | 6730 | Cuc(Cuc), | |
| 86 | 6730 | Cis(Cis), | |
| 87 | 6730 | Cic(Cic), | |
| 88 | 6730 | Crs(Crs), | |
| 89 | 6730 | Crc(Crc), | |
| 90 | 6730 | svAccuracy(svAccuracy), | |
| 91 | 6730 | satH1(static_cast<uint8_t>(satH1)), | |
| 92 | 6730 | T_GD1(T_GD1), | |
| 93 |
1/2✓ Branch 2 taken 6730 times.
✗ Branch 3 not taken.
|
13460 | T_GD2(T_GD2) |
| 94 | 6730 | {} | |
| 95 | |||
| 96 | #endif | ||
| 97 | |||
| 98 | 2195 | Clock::Corrections BDSEphemeris::calcClockCorrections(const InsTime& recvTime, double dist, const Frequency& freq) const | |
| 99 | { | ||
| 100 | LOG_DATA("Calc Sat Clock corrections at receiver time {}", recvTime.toGPSweekTow(BDT)); | ||
| 101 | // Earth gravitational constant [m³/s²] (WGS 84 value of the earth's gravitational constant for GPS user) | ||
| 102 | 2195 | const auto mu = InsConst::BDS::MU; | |
| 103 | // Relativistic constant F for clock corrections [s/√m] (-2*√µ/c²) | ||
| 104 | 2195 | const auto F = InsConst::BDS::F; | |
| 105 | |||
| 106 | LOG_DATA(" toe {} (Time of ephemeris)", toe.toGPSweekTow(BDT)); | ||
| 107 | |||
| 108 | 2195 | const auto A = sqrt_A * sqrt_A; // Semi-major axis [m] | |
| 109 | LOG_DATA(" A {} [m] (Semi-major axis)", A); | ||
| 110 | 2195 | auto n_0 = std::sqrt(mu / std::pow(A, 3)); // Computed mean motion [rad/s] | |
| 111 | LOG_DATA(" n_0 {} [rad/s] (Computed mean motion)", n_0); | ||
| 112 | 2195 | auto n = n_0 + delta_n; // Corrected mean motion [rad/s] | |
| 113 | LOG_DATA(" n {} [rad/s] (Corrected mean motion)", n); | ||
| 114 | |||
| 115 | // Time at transmission | ||
| 116 |
2/4✓ Branch 2 taken 2195 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2195 times.
✗ Branch 6 not taken.
|
2195 | InsTime transTime0 = recvTime - std::chrono::duration<double>(dist / InsConst::C); |
| 117 | |||
| 118 | 2195 | InsTime transTime = transTime0; | |
| 119 | LOG_DATA(" Iterating Time at transmission"); | ||
| 120 | 2195 | double dt_sv = 0.0; | |
| 121 | 2195 | double clkDrift = 0.0; | |
| 122 | |||
| 123 |
2/2✓ Branch 0 taken 4390 times.
✓ Branch 1 taken 2195 times.
|
6585 | for (size_t i = 0; i < 2; i++) |
| 124 | { | ||
| 125 | LOG_DATA(" transTime {} (Time at transmission)", transTime.toGPSweekTow(BDT)); | ||
| 126 | |||
| 127 | // [s] | ||
| 128 |
1/2✓ Branch 1 taken 4390 times.
✗ Branch 2 not taken.
|
4390 | auto t_minus_toc = static_cast<double>((transTime - toc).count()); |
| 129 | LOG_DATA(" transTime - toc {} [s]", t_minus_toc); | ||
| 130 | |||
| 131 | // Time difference from ephemeris reference epoch [s] | ||
| 132 |
1/2✓ Branch 1 taken 4390 times.
✗ Branch 2 not taken.
|
4390 | double t_k = static_cast<double>((transTime - toe).count()); |
| 133 | LOG_DATA(" transTime - toe {} [s] (t_k = Time difference from ephemeris reference epoch)", t_k); | ||
| 134 | |||
| 135 | // Mean anomaly [rad] | ||
| 136 | 4390 | auto M_k = M_0 + n * t_k; | |
| 137 | LOG_DATA(" M_k {} [s] (Mean anomaly)", M_k); | ||
| 138 | |||
| 139 | // Eccentric anomaly [rad] | ||
| 140 | 4390 | double E_k = M_k; | |
| 141 | 4390 | double E_k_old = 0.0; | |
| 142 | |||
| 143 |
5/6✓ Branch 1 taken 21716 times.
✓ Branch 2 taken 4390 times.
✓ Branch 3 taken 21716 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 21716 times.
✓ Branch 6 taken 4390 times.
|
26106 | for (size_t i = 0; std::abs(E_k - E_k_old) > 1e-13 && i < 10; i++) |
| 144 | { | ||
| 145 | 21716 | 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: | |
| 146 | 21716 | E_k = M_k + e * sin(E_k); | |
| 147 | } | ||
| 148 | |||
| 149 | // Relativistic correction term [s] | ||
| 150 | 4390 | double dt_r = F * e * sqrt_A * std::sin(E_k); | |
| 151 | LOG_DATA(" dt_r {} [s] (Relativistic correction term)", dt_r); | ||
| 152 | |||
| 153 | // SV PRN code phase time offset [s] | ||
| 154 | 4390 | dt_sv = a[0] + a[1] * t_minus_toc + a[2] * std::pow(t_minus_toc, 2) + dt_r; | |
| 155 | |||
| 156 | // See BDS-SIS-ICD-2.1 BDS ICD, ch. 5.2.4.10, p.31 | ||
| 157 |
4/4✓ Branch 1 taken 690 times.
✓ Branch 2 taken 3700 times.
✓ Branch 4 taken 690 times.
✓ Branch 5 taken 3010 times.
|
4390 | dt_sv -= (freq == B02 ? T_GD1 : freq == B07 ? T_GD2 // TODO: check again |
| 158 | : 0); | ||
| 159 | |||
| 160 | LOG_DATA(" dt_sv {} [s] (SV PRN code phase time offset)", dt_sv); | ||
| 161 | |||
| 162 | // Groves ch. 9.3.1, eq. 9.78, p. 391 | ||
| 163 | 4390 | clkDrift = a[1] + a[2] / 2.0 * t_minus_toc; | |
| 164 | |||
| 165 | // Correct transmit time for the satellite clock bias | ||
| 166 |
2/4✓ Branch 2 taken 4390 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4390 times.
✗ Branch 6 not taken.
|
4390 | transTime = transTime0 - std::chrono::duration<double>(dt_sv); |
| 167 | } | ||
| 168 | |||
| 169 | 2195 | return { .transmitTime = transTime, .bias = dt_sv, .drift = clkDrift }; | |
| 170 | } | ||
| 171 | |||
| 172 | 6385 | Orbit::PosVelAccel BDSEphemeris::calcSatelliteData(const InsTime& transTime, Orbit::Calc calc) const | |
| 173 | { | ||
| 174 |
2/4✓ Branch 1 taken 6385 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6385 times.
✗ Branch 5 not taken.
|
6385 | Eigen::Vector3d e_pos = Eigen::Vector3d::Zero(); |
| 175 |
2/4✓ Branch 1 taken 6385 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6385 times.
✗ Branch 5 not taken.
|
6385 | Eigen::Vector3d e_vel = Eigen::Vector3d::Zero(); |
| 176 |
2/4✓ Branch 1 taken 6385 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6385 times.
✗ Branch 5 not taken.
|
6385 | Eigen::Vector3d e_accel = Eigen::Vector3d::Zero(); |
| 177 | |||
| 178 | LOG_DATA("Calc Sat Position at transmit time {}", transTime.toGPSweekTow(BDT)); | ||
| 179 | // Earth gravitational constant [m³/s²] (WGS 84 value of the earth's gravitational constant for GPS user) | ||
| 180 | 6385 | const auto mu = InsConst::BDS::MU; | |
| 181 | // Earth angular velocity [rad/s] (WGS 84 value of the earth's rotation rate) | ||
| 182 | 6385 | const auto Omega_e_dot = InsConst::BDS::omega_ie; | |
| 183 | |||
| 184 | LOG_DATA(" toe {} (Time of ephemeris)", toe.toGPSweekTow(BDT)); | ||
| 185 | |||
| 186 | 6385 | const auto A = sqrt_A * sqrt_A; // Semi-major axis [m] | |
| 187 | LOG_DATA(" A {} [m] (Semi-major axis)", A); | ||
| 188 | 6385 | auto n_0 = std::sqrt(mu / std::pow(A, 3)); // Computed mean motion [rad/s] | |
| 189 | LOG_DATA(" n_0 {} [rad/s] (Computed mean motion)", n_0); | ||
| 190 | 6385 | auto n = n_0 + delta_n; // Corrected mean motion [rad/s] | |
| 191 | LOG_DATA(" n {} [rad/s] (Corrected mean motion)", n); | ||
| 192 | |||
| 193 | // Eccentric anomaly [rad] | ||
| 194 | 6385 | double E_k = 0.0; | |
| 195 | |||
| 196 | // Computed time from ephemeris reference epoch [s] | ||
| 197 |
1/2✓ Branch 1 taken 6385 times.
✗ Branch 2 not taken.
|
6385 | double t_k = static_cast<double>((transTime - toe).count()); |
| 198 | LOG_DATA(" t_k {} [s] (Time difference from ephemeris reference epoch)", t_k); | ||
| 199 | |||
| 200 | // Computed Mean anomaly [rad] | ||
| 201 | 6385 | auto M_k = M_0 + n * t_k; | |
| 202 | LOG_DATA(" M_k {} [s] (Mean anomaly)", M_k); | ||
| 203 | |||
| 204 | 6385 | E_k = M_k; // Initial Value [rad] | |
| 205 | 6385 | double E_k_old = 0.0; | |
| 206 | LOG_DATA(" Iterating E_k"); | ||
| 207 | LOG_DATA(" E_k {} [rad] (Eccentric anomaly)", E_k); | ||
| 208 |
5/6✓ Branch 1 taken 18357 times.
✓ Branch 2 taken 6385 times.
✓ Branch 3 taken 18357 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 18357 times.
✓ Branch 6 taken 6385 times.
|
24742 | for (size_t i = 0; std::abs(E_k - E_k_old) > 1e-13 && i < 10; i++) |
| 209 | { | ||
| 210 | 18357 | 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: | |
| 211 | 18357 | 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) | |
| 212 | LOG_DATA(" E_k {} [rad] (Eccentric anomaly)", E_k); // – Final Value (radians) | ||
| 213 | } | ||
| 214 | |||
| 215 | // 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] | ||
| 216 | 6385 | 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 | |
| 217 | LOG_DATA(" v_k {} [rad] (True Anomaly (unambiguous quadrant))", v_k); | ||
| 218 | |||
| 219 | 6385 | auto Phi_k = v_k + omega; // Computed Argument of Latitude [rad] | |
| 220 | LOG_DATA(" Phi_k {} [rad] (Argument of Latitude)", Phi_k); | ||
| 221 | |||
| 222 | // Second Harmonic Perturbations | ||
| 223 | 6385 | auto delta_u_k = Cus * std::sin(2 * Phi_k) + Cuc * std::cos(2 * Phi_k); // Argument of Latitude Correction [rad] | |
| 224 | LOG_DATA(" delta_u_k {} [rad] (Argument of Latitude Correction)", delta_u_k); | ||
| 225 | 6385 | auto delta_r_k = Crs * std::sin(2 * Phi_k) + Crc * std::cos(2 * Phi_k); // Radius Correction [m] | |
| 226 | LOG_DATA(" delta_r_k {} [m] (Radius Correction)", delta_r_k); | ||
| 227 | 6385 | auto delta_i_k = Cis * std::sin(2 * Phi_k) + Cic * std::cos(2 * Phi_k); // Inclination Correction [rad] | |
| 228 | LOG_DATA(" delta_i_k {} [rad] (Inclination Correction)", delta_i_k); | ||
| 229 | |||
| 230 | 6385 | auto u_k = Phi_k + delta_u_k; // Corrected Argument of Latitude [rad] | |
| 231 | LOG_DATA(" u_k {} [rad] (Corrected Argument of Latitude)", u_k); | ||
| 232 | 6385 | auto r_k = A * (1 - e * std::cos(E_k)) + delta_r_k; // Corrected Radius [m] | |
| 233 | LOG_DATA(" r_k {} [m] (Corrected Radius)", r_k); | ||
| 234 | 6385 | auto i_k = i_0 + delta_i_k + i_dot * t_k; // Corrected Inclination [rad] | |
| 235 | LOG_DATA(" i_k {} [rad] (Corrected Inclination)", i_k); | ||
| 236 | |||
| 237 | 6385 | auto x_k_op = r_k * std::cos(u_k); // Computed position in orbital plane [m] | |
| 238 | LOG_DATA(" x_k_op {} [m] (Position in orbital plane)", x_k_op); | ||
| 239 | 6385 | auto y_k_op = r_k * std::sin(u_k); // Computed position in orbital plane [m] | |
| 240 | LOG_DATA(" y_k_op {} [m] (Position in orbital plane)", y_k_op); | ||
| 241 | |||
| 242 | 6385 | double Omega_k = 0.0; | |
| 243 | 6385 | double x_k = 0.0; | |
| 244 | 6385 | double y_k = 0.0; | |
| 245 | 6385 | double z_k = 0.0; | |
| 246 | |||
| 247 |
3/4✓ Branch 3 taken 6385 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 50 times.
✓ Branch 6 taken 6335 times.
|
6385 | if (SatId(BDS, satNum).isGeo()) |
| 248 | { | ||
| 249 | // Corrected longitude of ascending node [rad] | ||
| 250 |
1/2✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
|
50 | Omega_k = Omega_0 + Omega_dot * t_k - Omega_e_dot * static_cast<double>(toe.toGPSweekTow(BDT).tow); |
| 251 | LOG_DATA(" Omega_k {} [rad] (Corrected longitude of ascending node)", Omega_k); | ||
| 252 | |||
| 253 |
1/2✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
|
50 | Eigen::Vector3d X_GK{ 0, 0, 0 }; |
| 254 | |||
| 255 |
1/2✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
|
50 | X_GK(0) = x_k_op * std::cos(Omega_k) - y_k_op * std::cos(i_k) * std::sin(Omega_k); |
| 256 |
1/2✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
|
50 | X_GK(1) = x_k_op * std::sin(Omega_k) + y_k_op * std::cos(i_k) * std::cos(Omega_k); |
| 257 |
1/2✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
|
50 | X_GK(2) = y_k_op * std::sin(i_k); |
| 258 | |||
| 259 | 50 | auto Rx = [](double phi) -> Eigen::Matrix3d { | |
| 260 | 50 | Eigen::Matrix3d C; | |
| 261 |
3/6✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
|
50 | C << 1, 0, 0, |
| 262 |
3/6✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
|
50 | 0, std::cos(phi), std::sin(phi), |
| 263 |
3/6✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
|
50 | 0, -std::sin(phi), std::cos(phi); |
| 264 | 50 | return C; | |
| 265 | }; | ||
| 266 | 50 | auto Rz = [](double phi) -> Eigen::Matrix3d { | |
| 267 | 50 | Eigen::Matrix3d C; | |
| 268 |
3/6✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
|
50 | C << std::cos(phi), std::sin(phi), 0, |
| 269 |
3/6✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
|
50 | -std::sin(phi), std::cos(phi), 0, |
| 270 |
3/6✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
|
50 | 0, 0, 1; |
| 271 | 50 | return C; | |
| 272 | }; | ||
| 273 | |||
| 274 |
5/10✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 50 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 50 times.
✗ Branch 15 not taken.
|
50 | e_pos = Rz(Omega_e_dot * t_k) * Rx(deg2rad(-5)) * X_GK; |
| 275 | |||
| 276 | return { .e_pos = e_pos, | ||
| 277 | ✗ | .e_vel = Eigen::Vector3d::Zero(), | |
| 278 |
5/10✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 50 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 50 times.
✗ Branch 14 not taken.
|
100 | .e_accel = Eigen::Vector3d::Zero() }; |
| 279 | } | ||
| 280 | // Satellite has a MEO or IGSO orbit | ||
| 281 | |||
| 282 | // Corrected longitude of ascending node [rad] | ||
| 283 |
1/2✓ Branch 2 taken 6335 times.
✗ Branch 3 not taken.
|
6335 | Omega_k = Omega_0 + (Omega_dot - Omega_e_dot) * t_k - Omega_e_dot * static_cast<double>(toe.toGPSweekTow(BDT).tow); |
| 284 | LOG_DATA(" Omega_k {} [rad] (Corrected longitude of ascending node)", Omega_k); | ||
| 285 | |||
| 286 | // Earth-fixed x coordinates [m] | ||
| 287 | 6335 | x_k = x_k_op * std::cos(Omega_k) - y_k_op * std::cos(i_k) * std::sin(Omega_k); | |
| 288 | LOG_DATA(" x_k {} [m] (Earth-fixed x coordinates)", x_k); | ||
| 289 | // Earth-fixed y coordinates [m] | ||
| 290 | 6335 | y_k = x_k_op * std::sin(Omega_k) + y_k_op * std::cos(i_k) * std::cos(Omega_k); | |
| 291 | LOG_DATA(" y_k {} [m] (Earth-fixed y coordinates)", y_k); | ||
| 292 | // Earth-fixed z coordinates [m] | ||
| 293 | 6335 | z_k = y_k_op * std::sin(i_k); | |
| 294 | LOG_DATA(" z_k {} [m] (Earth-fixed z coordinates)", z_k); | ||
| 295 | |||
| 296 |
1/2✓ Branch 1 taken 6335 times.
✗ Branch 2 not taken.
|
6335 | e_pos = Eigen::Vector3d{ x_k, y_k, z_k }; |
| 297 | |||
| 298 |
5/6✓ Branch 1 taken 2145 times.
✓ Branch 2 taken 4190 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 2145 times.
✓ Branch 6 taken 4190 times.
✓ Branch 7 taken 2145 times.
|
6335 | if (calc & Calc_Velocity || calc & Calc_Acceleration) |
| 299 | { | ||
| 300 | // Eccentric Anomaly Rate [rad/s] | ||
| 301 | 4190 | auto E_k_dot = n / (1 - e * std::cos(E_k)); | |
| 302 | // True Anomaly Rate [rad/s] | ||
| 303 | 4190 | auto v_k_dot = E_k_dot * std::sqrt(1 - e * e) / (1 - e * std::cos(E_k)); | |
| 304 | // Corrected Inclination Angle Rate [rad/s] | ||
| 305 | 4190 | auto i_k_dot = i_dot + 2 * v_k_dot * (Cis * std::cos(2 * Phi_k) - Cic * std::sin(2 * Phi_k)); | |
| 306 | // Corrected Argument of Latitude Rate [rad/s] | ||
| 307 | 4190 | auto u_k_dot = v_k_dot + 2 * v_k_dot * (Cus * std::cos(2 * Phi_k) - Cuc * std::sin(2 * Phi_k)); | |
| 308 | // Corrected Radius Rate [m/s] | ||
| 309 | 4190 | 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)); | |
| 310 | // Longitude of Ascending Node Rate [rad/s] | ||
| 311 | 4190 | auto Omega_k_dot = Omega_dot - Omega_e_dot; | |
| 312 | // In-plane x velocity [m/s] | ||
| 313 | 4190 | auto vx_k_op = r_k_dot * std::cos(u_k) - r_k * u_k_dot * std::sin(u_k); | |
| 314 | // In-plane y velocity [m/s] | ||
| 315 | 4190 | auto vy_k_op = r_k_dot * std::sin(u_k) + r_k * u_k_dot * std::cos(u_k); | |
| 316 | // Earth-Fixed x velocity [m/s] | ||
| 317 | 4190 | 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) | |
| 318 | 4190 | - 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)); | |
| 319 | // Earth-Fixed y velocity [m/s] | ||
| 320 | 4190 | 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) | |
| 321 | 4190 | - 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)); | |
| 322 | // Earth-Fixed z velocity [m/s] | ||
| 323 | 4190 | auto vz_k = vy_k_op * std::sin(i_k) + y_k_op * i_k_dot * std::cos(i_k); | |
| 324 | |||
| 325 |
1/2✓ Branch 1 taken 4190 times.
✗ Branch 2 not taken.
|
4190 | if (calc & Calc_Velocity) |
| 326 | { | ||
| 327 |
1/2✓ Branch 1 taken 4190 times.
✗ Branch 2 not taken.
|
4190 | e_vel = Eigen::Vector3d{ vx_k, vy_k, vz_k }; |
| 328 | } | ||
| 329 | |||
| 330 |
2/2✓ Branch 1 taken 2095 times.
✓ Branch 2 taken 2095 times.
|
4190 | if (calc & Calc_Acceleration) |
| 331 | { | ||
| 332 | // Oblate Earth acceleration Factor [m/s^2] | ||
| 333 | 2095 | auto F = -(3.0 / 2.0) * InsConst::GPS::J2 * (mu / std::pow(r_k, 2)) * std::pow(InsConst::GPS::R_E / r_k, 2); | |
| 334 | // Earth-Fixed x acceleration [m/s^2] | ||
| 335 | 2095 | 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)) | |
| 336 | 2095 | + 2 * vy_k * Omega_e_dot + x_k * std::pow(Omega_e_dot, 2); | |
| 337 | // Earth-Fixed y acceleration [m/s^2] | ||
| 338 | 2095 | 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)) | |
| 339 | 2095 | + 2 * vx_k * Omega_e_dot + y_k * std::pow(Omega_e_dot, 2); | |
| 340 | // Earth-Fixed z acceleration [m/s^2] | ||
| 341 | 2095 | 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)); | |
| 342 | |||
| 343 |
1/2✓ Branch 1 taken 2095 times.
✗ Branch 2 not taken.
|
2095 | e_accel = Eigen::Vector3d{ ax_k, ay_k, az_k }; |
| 344 | } | ||
| 345 | } | ||
| 346 | |||
| 347 | return { .e_pos = e_pos, | ||
| 348 | .e_vel = e_vel, | ||
| 349 |
3/6✓ Branch 1 taken 6335 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6335 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6335 times.
✗ Branch 8 not taken.
|
6335 | .e_accel = e_accel }; |
| 350 | } | ||
| 351 | |||
| 352 | 2070 | bool BDSEphemeris::isHealthy() const | |
| 353 | { | ||
| 354 | 2070 | return satH1 == 0; | |
| 355 | } | ||
| 356 | |||
| 357 | ✗ | double BDSEphemeris::calcSatellitePositionVariance() const | |
| 358 | { | ||
| 359 | ✗ | return std::pow(svAccuracy, 2); | |
| 360 | } | ||
| 361 | |||
| 362 | } // namespace NAV | ||
| 363 |