INSTINCT Code Coverage Report

Directory: src/
File: Navigation/GNSS/Satellite/Ephemeris/QZSSEphemeris.cpp
Date: 2025-07-19 10:51:51
Exec Total Coverage
Lines: 140 144 97.2%
Functions: 4 6 66.7%
Branches: 47 76 61.8%
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 "QZSSEphemeris.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 4 QZSSEphemeris::QZSSEphemeris(const InsTime& toc, const InsTime& toe,
20 const size_t& IODE, const size_t& IODC,
21 const std::array<double, 3>& a,
22 const double& sqrt_A, const double& e, const double& i_0, const double& Omega_0, const double& omega, const double& M_0,
23 const double& delta_n, const double& Omega_dot, const double& i_dot, const double& Cus, const double& Cuc,
24 const double& Cis, const double& Cic, const double& Crs, const double& Crc,
25 const double& svAccuracy, uint8_t svHealth,
26 uint8_t L2ChannelCodes, bool L2DataFlagPCode,
27 4 const double& T_GD, bool fitIntervalFlag)
28 : SatNavData(SatNavData::QZSSEphemeris, toc),
29 4 toc(toc),
30 4 toe(toe),
31 4 IODE(IODE),
32 4 IODC(IODC),
33 4 a(a),
34 4 sqrt_A(sqrt_A),
35 4 e(e),
36 4 i_0(i_0),
37 4 Omega_0(Omega_0),
38 4 omega(omega),
39 4 M_0(M_0),
40 4 delta_n(delta_n),
41 4 Omega_dot(Omega_dot),
42 4 i_dot(i_dot),
43 4 Cus(Cus),
44 4 Cuc(Cuc),
45 4 Cis(Cis),
46 4 Cic(Cic),
47 4 Crs(Crs),
48 4 Crc(Crc),
49 4 svAccuracy(svAccuracy),
50 4 svHealth(svHealth),
51 4 L2ChannelCodes(L2ChannelCodes),
52 4 L2DataFlagPCode(L2DataFlagPCode),
53 4 T_GD(T_GD),
54 4 fitIntervalFlag(fitIntervalFlag) {}
55
56 #ifdef TESTING
57
58 1186 QZSSEphemeris::QZSSEphemeris(int32_t year, int32_t month, int32_t day, int32_t hour, int32_t minute, double second, double svClockBias, double svClockDrift, double svClockDriftRate,
59 double IODE, double Crs, double delta_n, double M_0,
60 double Cuc, double e, double Cus, double sqrt_A,
61 double Toe, double Cic, double Omega_0, double Cis,
62 double i_0, double Crc, double omega, double Omega_dot,
63 double i_dot, double L2ChannelCodes, double GPSWeek, double L2DataFlagPCode,
64 double svAccuracy, double svHealth, double T_GD, double IODC,
65 1186 double /* TransmissionTimeOfMessage */, double fitIntervalFlag, double /* spare1 */, double /* spare2 */)
66
2/4
✓ Branch 1 taken 1186 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1186 times.
✗ Branch 5 not taken.
 1186 : SatNavData(SatNavData::QZSSEphemeris, InsTime(year, month, day, hour, minute, second, SatelliteSystem(QZSS).getTimeSystem())),
67 1186 toc(refTime),
68
2/4
✓ Branch 1 taken 1186 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1186 times.
✗ Branch 5 not taken.
 1186 toe(InsTime(0, static_cast<int32_t>(GPSWeek), Toe, SatelliteSystem(QZSS).getTimeSystem())),
69 1186 IODE(static_cast<size_t>(IODE)),
70 1186 IODC(static_cast<size_t>(IODC)),
71 1186 a({ svClockBias, svClockDrift, svClockDriftRate }),
72 1186 sqrt_A(sqrt_A),
73 1186 e(e),
74 1186 i_0(i_0),
75 1186 Omega_0(Omega_0),
76 1186 omega(omega),
77 1186 M_0(M_0),
78 1186 delta_n(delta_n),
79 1186 Omega_dot(Omega_dot),
80 1186 i_dot(i_dot),
81 1186 Cus(Cus),
82 1186 Cuc(Cuc),
83 1186 Cis(Cis),
84 1186 Cic(Cic),
85 1186 Crs(Crs),
86 1186 Crc(Crc),
87 1186 svAccuracy(svAccuracy),
88 1186 svHealth(static_cast<uint8_t>(svHealth)),
89 1186 L2ChannelCodes(static_cast<uint8_t>(L2ChannelCodes)),
90 1186 L2DataFlagPCode(static_cast<bool>(L2DataFlagPCode)),
91 1186 T_GD(T_GD),
92
1/2
✓ Branch 2 taken 1186 times.
✗ Branch 3 not taken.
 2372 fitIntervalFlag(static_cast<bool>(fitIntervalFlag))
93 1186 {}
94
95 #endif
96
97 150 Clock::Corrections QZSSEphemeris::calcClockCorrections(const InsTime& recvTime, double dist, const Frequency& freq) const
98 {
99 LOG_DATA("Calc Sat Clock corrections at receiver time {}", recvTime.toGPSweekTow());
100 // Earth gravitational constant [m³/s²] (WGS 84 value of the earth's gravitational constant for GPS user)
101 150 const auto mu = InsConst::QZSS::MU;
102 // Relativistic constant F for clock corrections [s/√m] (-2*√µ/c²)
103 150 const auto F = InsConst::QZSS::F;
104
105 LOG_DATA(" toe {} (Time of ephemeris)", toe.toGPSweekTow());
106
107 150 const auto A = sqrt_A * sqrt_A; // Semi-major axis [m]
108 LOG_DATA(" A {} [m] (Semi-major axis)", A);
109 150 auto n_0 = std::sqrt(mu / std::pow(A, 3)); // Computed mean motion [rad/s]
110 LOG_DATA(" n_0 {} [rad/s] (Computed mean motion)", n_0);
111 150 auto n = n_0 + delta_n; // Corrected mean motion [rad/s]
112 LOG_DATA(" n {} [rad/s] (Corrected mean motion)", n);
113
114 // Time at transmission
115
2/4
✓ Branch 2 taken 150 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 150 times.
✗ Branch 6 not taken.
 150 InsTime transTime0 = recvTime - std::chrono::duration<double>(dist / InsConst::C);
116
117 150 InsTime transTime = transTime0;
118 LOG_DATA(" Iterating Time at transmission");
119 150 double dt_sv = 0.0;
120 150 double clkDrift = 0.0;
121
122
2/2
✓ Branch 0 taken 300 times.
✓ Branch 1 taken 150 times.
 450 for (size_t i = 0; i < 2; i++)
123 {
124 LOG_DATA(" transTime {} (Time at transmission)", transTime.toGPSweekTow());
125
126 // [s]
127
1/2
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
 300 auto t_minus_toc = static_cast<double>((transTime - toc).count());
128 LOG_DATA(" transTime - toc {} [s]", t_minus_toc);
129
130 // Time difference from ephemeris reference epoch [s]
131
1/2
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
 300 double t_k = static_cast<double>((transTime - toe).count());
132 LOG_DATA(" transTime - toe {} [s] (t_k = Time difference from ephemeris reference epoch)", t_k);
133
134 // Mean anomaly [rad]
135 300 auto M_k = M_0 + n * t_k;
136 LOG_DATA(" M_k {} [s] (Mean anomaly)", M_k);
137
138 // Eccentric anomaly [rad]
139 300 double E_k = M_k;
140 300 double E_k_old = 0.0;
141
142
6/6
✓ Branch 1 taken 2904 times.
✓ Branch 2 taken 150 times.
✓ Branch 3 taken 2754 times.
✓ Branch 4 taken 150 times.
✓ Branch 5 taken 2754 times.
✓ Branch 6 taken 300 times.
 3054 for (size_t i = 0; std::abs(E_k - E_k_old) > 1e-13 && i < 10; i++)
143 {
144 2754 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:
145 2754 E_k = M_k + e * sin(E_k);
146 }
147
148 // Relativistic correction term [s]
149 300 double dt_r = F * e * sqrt_A * std::sin(E_k);
150 LOG_DATA(" dt_r {} [s] (Relativistic correction term)", dt_r);
151
152 // SV PRN code phase time offset [s]
153 300 dt_sv = a[0] + a[1] * t_minus_toc + a[2] * std::pow(t_minus_toc, 2) + dt_r;
154
155 // See IS-GPS-200M GPS ICD, ch. 20.3.3.3.3.2, p.102
156
1/2
✓ Branch 2 taken 300 times.
✗ Branch 3 not taken.
 300 dt_sv -= ratioFreqSquared(J01, freq, -128, -128) * T_GD; // TODO: Check again
157
158 LOG_DATA(" dt_sv {} [s] (SV PRN code phase time offset)", dt_sv);
159
160 // Groves ch. 9.3.1, eq. 9.78, p. 391
161 300 clkDrift = a[1] + a[2] / 2.0 * t_minus_toc;
162
163 // Correct transmit time for the satellite clock bias
164
2/4
✓ Branch 2 taken 300 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 300 times.
✗ Branch 6 not taken.
 300 transTime = transTime0 - std::chrono::duration<double>(dt_sv);
165 }
166 LOG_DATA(" transTime {} (Time at transmission)", transTime.toGPSweekTow());
167
168 150 return { .transmitTime = transTime, .bias = dt_sv, .drift = clkDrift };
169 }
170
171 250 Orbit::PosVelAccel QZSSEphemeris::calcSatelliteData(const InsTime& transTime, Orbit::Calc calc) const
172 {
173
2/4
✓ Branch 1 taken 250 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 250 times.
✗ Branch 5 not taken.
 250 Eigen::Vector3d e_pos = Eigen::Vector3d::Zero();
174
2/4
✓ Branch 1 taken 250 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 250 times.
✗ Branch 5 not taken.
 250 Eigen::Vector3d e_vel = Eigen::Vector3d::Zero();
175
2/4
✓ Branch 1 taken 250 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 250 times.
✗ Branch 5 not taken.
 250 Eigen::Vector3d e_accel = Eigen::Vector3d::Zero();
176
177 LOG_DATA("Calc Sat Position at transmit time {}", transTime.toGPSweekTow());
178 // Earth gravitational constant [m³/s²] (WGS 84 value of the earth's gravitational constant for GPS user)
179 250 const auto mu = InsConst::QZSS::MU;
180 // Earth angular velocity [rad/s] (WGS 84 value of the earth's rotation rate)
181 250 const auto Omega_e_dot = InsConst::QZSS::omega_ie;
182
183 LOG_DATA(" toe {} (Time of ephemeris)", toe.toGPSweekTow());
184
185 250 const auto A = sqrt_A * sqrt_A; // Semi-major axis [m]
186 LOG_DATA(" A {} [m] (Semi-major axis)", A);
187 250 auto n_0 = std::sqrt(mu / std::pow(A, 3)); // Computed mean motion [rad/s]
188 LOG_DATA(" n_0 {} [rad/s] (Computed mean motion)", n_0);
189 250 auto n = n_0 + delta_n; // Corrected mean motion [rad/s]
190 LOG_DATA(" n {} [rad/s] (Corrected mean motion)", n);
191
192 // Eccentric anomaly [rad]
193 250 double E_k = 0.0;
194
195 // Time difference from ephemeris reference epoch [s]
196
1/2
✓ Branch 1 taken 250 times.
✗ Branch 2 not taken.
 250 double t_k = static_cast<double>((transTime - toe).count());
197 LOG_DATA(" t_k {} [s] (Time difference from ephemeris reference epoch)", t_k);
198
199 // Mean anomaly [rad]
200 250 auto M_k = M_0 + n * t_k;
201 LOG_DATA(" M_k {} [s] (Mean anomaly)", M_k);
202
203 250 E_k = M_k; // Initial Value [rad]
204 250 double E_k_old = 0.0;
205 LOG_DATA(" Iterating E_k");
206 LOG_DATA(" E_k {} [rad] (Eccentric anomaly)", E_k);
207
5/6
✓ Branch 1 taken 970 times.
✓ Branch 2 taken 250 times.
✓ Branch 3 taken 970 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 970 times.
✓ Branch 6 taken 250 times.
 1220 for (size_t i = 0; std::abs(E_k - E_k_old) > 1e-13 && i < 10; i++)
208 {
209 970 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:
210 970 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)
211 LOG_DATA(" E_k {} [rad] (Eccentric anomaly)", E_k); // - Final Value (radians)
212 }
213
214 // 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)
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] (GALILEO ICD algorithm)
216 250 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 250 auto Phi_k = v_k + omega; // Argument of Latitude [rad]
219 LOG_DATA(" Phi_k {} [rad] (Argument of Latitude)", Phi_k);
220
221 // Second Harmonic Perturbations
222 250 auto delta_u_k = Cus * std::sin(2 * Phi_k) + Cuc * std::cos(2 * Phi_k); // Argument of Latitude Correction [rad]
223 LOG_DATA(" delta_u_k {} [rad] (Argument of Latitude Correction)", delta_u_k);
224 250 auto delta_r_k = Crs * std::sin(2 * Phi_k) + Crc * std::cos(2 * Phi_k); // Radius Correction [m]
225 LOG_DATA(" delta_r_k {} [m] (Radius Correction)", delta_r_k);
226 250 auto delta_i_k = Cis * std::sin(2 * Phi_k) + Cic * std::cos(2 * Phi_k); // Inclination Correction [rad]
227 LOG_DATA(" delta_i_k {} [rad] (Inclination Correction)", delta_i_k);
228
229 250 auto u_k = Phi_k + delta_u_k; // Corrected Argument of Latitude [rad]
230 LOG_DATA(" u_k {} [rad] (Corrected Argument of Latitude)", u_k);
231 250 auto r_k = A * (1 - e * std::cos(E_k)) + delta_r_k; // Corrected Radius [m]
232 LOG_DATA(" r_k {} [m] (Corrected Radius)", r_k);
233 250 auto i_k = i_0 + delta_i_k + i_dot * t_k; // Corrected Inclination [rad]
234 LOG_DATA(" i_k {} [rad] (Corrected Inclination)", i_k);
235
236 250 auto x_k_op = r_k * std::cos(u_k); // Position in orbital plane [m]
237 LOG_DATA(" x_k_op {} [m] (Position in orbital plane)", x_k_op);
238 250 auto y_k_op = r_k * std::sin(u_k); // Position in orbital plane [m]
239 LOG_DATA(" y_k_op {} [m] (Position in orbital plane)", y_k_op);
240
241 // Corrected longitude of ascending node [rad]
242
1/2
✓ Branch 2 taken 250 times.
✗ Branch 3 not taken.
 250 auto Omega_k = Omega_0 + (Omega_dot - Omega_e_dot) * t_k - Omega_e_dot * static_cast<double>(toe.toGPSweekTow().tow);
243 LOG_DATA(" Omega_k {} [rad] (Corrected longitude of ascending node)", Omega_k);
244
245 // Earth-fixed x coordinates [m]
246 250 auto x_k = x_k_op * std::cos(Omega_k) - y_k_op * std::cos(i_k) * std::sin(Omega_k);
247 LOG_DATA(" x_k {} [m] (Earth-fixed x coordinates)", x_k);
248 // Earth-fixed y coordinates [m]
249 250 auto y_k = x_k_op * std::sin(Omega_k) + y_k_op * std::cos(i_k) * std::cos(Omega_k);
250 LOG_DATA(" y_k {} [m] (Earth-fixed y coordinates)", y_k);
251 // Earth-fixed z coordinates [m]
252 250 auto z_k = y_k_op * std::sin(i_k);
253 LOG_DATA(" z_k {} [m] (Earth-fixed z coordinates)", z_k);
254
255
1/2
✓ Branch 1 taken 250 times.
✗ Branch 2 not taken.
 250 e_pos = Eigen::Vector3d{ x_k, y_k, z_k };
256
257
5/6
✓ Branch 1 taken 150 times.
✓ Branch 2 taken 100 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 150 times.
✓ Branch 6 taken 100 times.
✓ Branch 7 taken 150 times.
 250 if (calc & Calc_Velocity || calc & Calc_Acceleration)
258 {
259 // Eccentric Anomaly Rate [rad/s]
260 100 auto E_k_dot = n / (1 - e * std::cos(E_k));
261 // True Anomaly Rate [rad/s]
262 100 auto v_k_dot = E_k_dot * std::sqrt(1 - e * e) / (1 - e * std::cos(E_k));
263 // Corrected Inclination Angle Rate [rad/s]
264 100 auto i_k_dot = i_dot + 2 * v_k_dot * (Cis * std::cos(2 * Phi_k) - Cic * std::sin(2 * Phi_k));
265 // Corrected Argument of Latitude Rate [rad/s]
266 100 auto u_k_dot = v_k_dot + 2 * v_k_dot * (Cus * std::cos(2 * Phi_k) - Cuc * std::sin(2 * Phi_k));
267 // Corrected Radius Rate [m/s]
268 100 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));
269 // Longitude of Ascending Node Rate [rad/s]
270 100 auto Omega_k_dot = Omega_dot - Omega_e_dot;
271 // In-plane x velocity [m/s]
272 100 auto vx_k_op = r_k_dot * std::cos(u_k) - r_k * u_k_dot * std::sin(u_k);
273 // In-plane y velocity [m/s]
274 100 auto vy_k_op = r_k_dot * std::sin(u_k) + r_k * u_k_dot * std::cos(u_k);
275 // Earth-Fixed x velocity [m/s]
276 100 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)
277 100 - 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));
278 // Earth-Fixed y velocity [m/s]
279 100 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)
280 100 - 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));
281 // Earth-Fixed z velocity [m/s]
282 100 auto vz_k = vy_k_op * std::sin(i_k) + y_k_op * i_k_dot * std::cos(i_k);
283
284
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 100 if (calc & Calc_Velocity)
285 {
286
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 100 e_vel = Eigen::Vector3d{ vx_k, vy_k, vz_k };
287 }
288
289
2/2
✓ Branch 1 taken 50 times.
✓ Branch 2 taken 50 times.
 100 if (calc & Calc_Acceleration)
290 {
291 // Oblate Earth acceleration Factor [m/s^2]
292 50 auto F = -(3.0 / 2.0) * InsConst::GPS::J2 * (mu / std::pow(r_k, 2)) * std::pow(InsConst::GPS::R_E / r_k, 2);
293 // Earth-Fixed x acceleration [m/s^2]
294 50 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))
295 50 + 2 * vy_k * Omega_e_dot + x_k * std::pow(Omega_e_dot, 2);
296 // Earth-Fixed y acceleration [m/s^2]
297 50 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))
298 50 + 2 * vx_k * Omega_e_dot + y_k * std::pow(Omega_e_dot, 2);
299 // Earth-Fixed z acceleration [m/s^2]
300 50 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));
301
302
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 e_accel = Eigen::Vector3d{ ax_k, ay_k, az_k };
303 }
304 }
305
306 return { .e_pos = e_pos,
307 .e_vel = e_vel,
308
3/6
✓ Branch 1 taken 250 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 250 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 250 times.
✗ Branch 8 not taken.
 500 .e_accel = e_accel };
309 }
310
311 ✗ bool QZSSEphemeris::isHealthy() const // TODO Parse Signal Id as a parameter and differentiate depending on the bitset
312 {
313 ✗ return svHealth.none();
314 }
315
316 ✗ double QZSSEphemeris::calcSatellitePositionVariance() const
317 {
318 // Getting the index and value again will discretize the URA values
319 ✗ return std::pow(gpsUraIdx2Val(gpsUraVal2Idx(svAccuracy)), 2);
320 }
321
322 } // namespace NAV
323