INSTINCT Code Coverage Report


Directory: src/
File: Navigation/GNSS/SystemModel/ReceiverClockModel.hpp
Date: 2025-06-06 13:17:47
Exec Total Coverage
Lines: 22 93 23.7%
Functions: 6 13 46.2%
Branches: 11 146 7.5%

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 /// @file ReceiverClockModel.hpp
10 /// @brief Receiver Clock System Model
11 /// @author T. Topp (topp@ins.uni-stuttgart.de)
12 /// @date 2024-08-20
13
14 #pragma once
15
16 #include <set>
17 #include <vector>
18
19 #include "Units.hpp"
20 #include "Navigation/GNSS/Core/SatelliteSystem.hpp"
21 #include "internal/gui/widgets/HelpMarker.hpp"
22 #include "util/Assert.h"
23 #include "util/Container/KeyedMatrix.hpp"
24 #include "Navigation/GNSS/SystemModel/SystemModel.hpp"
25 #include "Navigation/GNSS/SystemModel/Units.hpp"
26 #include "Navigation/Math/KalmanFilter.hpp"
27 #include "Navigation/Math/VanLoan.hpp"
28 #include "internal/gui/widgets/InputWithUnit.hpp"
29
30 namespace NAV
31 {
32 namespace Keys
33 {
34
35 /// @brief Receiver clock error [m]
36 struct RecvClkBias
37 {
38 /// @brief Equal comparison operator
39 /// @param rhs Right-hand side
40 29210 bool operator==(const RecvClkBias& rhs) const { return satSys == rhs.satSys; }
41 /// @brief Satellite system
42 SatelliteSystem satSys;
43 };
44 /// @brief Receiver clock drift [m/s]
45 struct RecvClkDrift
46 {
47 /// @brief Equal comparison operator
48 /// @param rhs Right-hand side
49 28339 bool operator==(const RecvClkDrift& rhs) const { return satSys == rhs.satSys; }
50 /// @brief Satellite system
51 SatelliteSystem satSys;
52 };
53
54 } // namespace Keys
55
56 /// Receiver Clock System Model
57 template<typename StateKeyType>
58 class ReceiverClockModel
59 {
60 public:
61 /// @brief Initializes the receiver clock model
62 21 void initialize()
63 {
64 // Covariance of the clock phase drift [m²/s]
65 21 _covarianceClkPhaseDrift = convertUnit(_gui_covarianceClkPhaseDrift, _gui_covarianceClkPhaseDriftUnit);
66 // Covariance of the frequency phase drift [m²/s³]
67 21 _covarianceClkFrequencyDrift = convertUnit(_gui_covarianceClkFrequencyDrift, _gui_covarianceClkFrequencyDriftUnit);
68 21 }
69
70 /// @brief Initializes the receiver clock model
71 /// @param[in, out] F System model matrix
72 /// @param[in, out] G Noise input matrix
73 /// @param[in, out] W Noise scale matrix
74 template<typename Scalar, int Size>
75 21 void initialize(KeyedMatrix<Scalar, StateKeyType, StateKeyType, Size, Size>& F,
76 KeyedMatrix<Scalar, StateKeyType, StateKeyType, Size, Size>& G,
77 KeyedMatrix<Scalar, StateKeyType, StateKeyType, Size, Size>& W)
78 {
79 21 initialize();
80
81
2/2
✓ Branch 6 taken 168 times.
✓ Branch 7 taken 21 times.
189 for (const auto& key : F.rowKeys())
82 {
83
2/2
✓ Branch 1 taken 21 times.
✓ Branch 2 taken 147 times.
168 if (const auto* biasKey = std::get_if<Keys::RecvClkBias>(&key))
84 {
85 21 auto driftKey = Keys::RecvClkDrift{ biasKey->satSys };
86
2/4
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 21 times.
✗ Branch 5 not taken.
21 INS_ASSERT_USER_ERROR(F.hasRow(driftKey), "The model should have bias and drift");
87
88
1/2
✓ Branch 3 taken 21 times.
✗ Branch 4 not taken.
21 F(*biasKey, driftKey) = 1;
89
1/2
✓ Branch 3 taken 21 times.
✗ Branch 4 not taken.
21 G(*biasKey, *biasKey) = 1;
90
1/2
✓ Branch 3 taken 21 times.
✗ Branch 4 not taken.
21 G(driftKey, driftKey) = 1;
91
1/2
✓ Branch 3 taken 21 times.
✗ Branch 4 not taken.
21 W(*biasKey, *biasKey) = _covarianceClkPhaseDrift;
92
1/2
✓ Branch 3 taken 21 times.
✗ Branch 4 not taken.
21 W(driftKey, driftKey) = _covarianceClkFrequencyDrift;
93 }
94 }
95 21 }
96
97 /// @brief Updates the provided Phi and Q matrix
98 /// @param[in, out] Phi State transition matrix
99 /// @param[in, out] Q System/Process noise covariance matrix
100 /// @param[in] F System model matrix
101 /// @param[in] G Noise input matrix
102 /// @param[in] W Noise scale matrix
103 /// @param[in] dt Time step size in [s]
104 /// @param[in] algorithm Algorithm to use for the calculation
105 template<typename Scalar, int Size>
106 void updatePhiAndQ(KeyedMatrix<Scalar, StateKeyType, StateKeyType, Size, Size>& Phi,
107 KeyedMatrix<Scalar, StateKeyType, StateKeyType, Size, Size>& Q,
108 const KeyedMatrix<Scalar, StateKeyType, StateKeyType, Size, Size>& F,
109 const KeyedMatrix<Scalar, StateKeyType, StateKeyType, Size, Size>& G,
110 const KeyedMatrix<Scalar, StateKeyType, StateKeyType, Size, Size>& W,
111 double dt,
112 SystemModelCalcAlgorithm algorithm) const
113 {
114 for (const auto& key : Phi.rowKeys())
115 {
116 if (const auto* bias = std::get_if<Keys::RecvClkBias>(&key))
117 {
118 auto drift = Keys::RecvClkDrift{ bias->satSys };
119 if (Phi.hasRow(drift))
120 {
121 std::vector<StateKeyType> keys = { *bias, drift };
122 if (algorithm == SystemModelCalcAlgorithm::VanLoan)
123 {
124 auto [PhiMot, QMot] = NAV::calcPhiAndQWithVanLoanMethod(
125 F.template block<2>(keys, keys),
126 G.template block<2>(keys, keys),
127 W.template block<2>(keys, keys),
128 dt);
129 Phi.template block<2>(keys, keys) = PhiMot;
130 Q.template block<2>(keys, keys) = QMot;
131 }
132 else // QCalculationAlgorithm::Taylor1
133 {
134 Phi.template block<2>(keys, keys) = transitionMatrix_Phi_Taylor(F.template block<2>(keys, keys), dt, 1);
135 Q.template block<2>(keys, keys) = calcProcessNoiseMatrixTaylor(dt, bias->satSys, keys)(all, all);
136 }
137 }
138 else
139 {
140 Phi(*bias, *bias) = 1;
141 Q(*bias, *bias) = _covarianceClkPhaseDrift * dt;
142 }
143 }
144 }
145 }
146
147 /// @brief Calculates the state transition matrix (𝚽) and the process noise covariance matrix (𝐐)
148 /// @param[in] dt Time step size in [s]
149 /// @param[in] satSys Satellite systems to use as keys
150 /// @param[in] algorithm Algorithm to use for the calculation
151 /// @return Phi and Q matrix
152 [[nodiscard]] std::pair<KeyedMatrix2d<StateKeyType>, KeyedMatrix2d<StateKeyType>>
153 calcPhiAndQ(double dt, SatelliteSystem satSys, SystemModelCalcAlgorithm algorithm)
154 {
155 std::vector<StateKeyType> keys = {
156 Keys::RecvClkBias{ satSys },
157 Keys::RecvClkDrift{ satSys },
158 };
159
160 KeyedMatrix2d<StateKeyType> F(Eigen::Matrix2d::Zero(), keys, keys);
161 KeyedMatrix2d<StateKeyType> G(Eigen::Matrix2d::Zero(), keys, keys);
162 KeyedMatrix2d<StateKeyType> W(Eigen::Matrix2d::Zero(), keys, keys);
163 initialize(F, G, W);
164
165 KeyedMatrix2d<StateKeyType> Phi(Eigen::Matrix2d::Zero(), keys, keys);
166 KeyedMatrix2d<StateKeyType> Q(Eigen::Matrix2d::Zero(), keys, keys);
167 updatePhiAndQ(Phi, Q, F, G, W, dt, algorithm);
168
169 return { Phi, Q };
170 }
171
172 /// @brief Shows a GUI
173 /// @param[in] itemWidth Width of the space for the config items
174 /// @param[in] unitWidth Width of the units
175 /// @param[in] id Unique id for ImGui
176 /// @return True if something was changed
177 bool ShowGui(float itemWidth, float unitWidth, const char* id)
178 {
179 bool changed = false;
180
181 if (gui::widgets::InputDoubleWithUnit(fmt::format("{} of the receiver clock phase drift (RW)##{}",
182 _gui_covarianceClkPhaseDriftUnit == Units::CovarianceClkPhaseDriftUnits::m_sqrts
183 ? "StdDev"
184 : "Variance",
185 id)
186 .c_str(),
187 itemWidth, unitWidth, &_gui_covarianceClkPhaseDrift,
188 _gui_covarianceClkPhaseDriftUnit,
189 MakeComboItems<Units::CovarianceClkPhaseDriftUnits>().c_str(),
190 0.0, 0.0, "%.2e", ImGuiInputTextFlags_CharsScientific))
191 {
192 LOG_DEBUG("{}: _gui_covarianceClkPhaseDrift changed to {}", id, _gui_covarianceClkPhaseDrift);
193 LOG_DEBUG("{}: _gui_covarianceClkPhaseDriftUnit changed to {}", id, to_string(_gui_covarianceClkPhaseDriftUnit));
194 changed = true;
195 }
196 ImGui::SameLine();
197 gui::widgets::HelpMarker(fmt::format("Typical values for a TCXO are {} {}",
198 _gui_covarianceClkPhaseDriftUnit == Units::CovarianceClkPhaseDriftUnits::m_sqrts ? 0.1 : 0.01,
199 to_string(_gui_covarianceClkPhaseDriftUnit))
200 .c_str());
201
202 if (gui::widgets::InputDoubleWithUnit(fmt::format("{} of the receiver clock frequency drift (IRW)##{}",
203 _gui_covarianceClkFrequencyDriftUnit == Units::CovarianceClkFrequencyDriftUnits::m_sqrts3
204 ? "StdDev"
205 : "Variance",
206 id)
207 .c_str(),
208 itemWidth, unitWidth, &_gui_covarianceClkFrequencyDrift,
209 _gui_covarianceClkFrequencyDriftUnit,
210 MakeComboItems<Units::CovarianceClkFrequencyDriftUnits>().c_str(),
211 0.0, 0.0, "%.2e", ImGuiInputTextFlags_CharsScientific))
212 {
213 LOG_DEBUG("{}: _gui_covarianceClkFrequencyDrift changed to {}", id, _gui_covarianceClkFrequencyDrift);
214 LOG_DEBUG("{}: _gui_covarianceClkFrequencyDriftUnit changed to {}", id, to_string(_gui_covarianceClkFrequencyDriftUnit));
215 changed = true;
216 }
217 ImGui::SameLine();
218 gui::widgets::HelpMarker(fmt::format("Typical values for a TCXO are {} {}",
219 _gui_covarianceClkFrequencyDriftUnit == Units::CovarianceClkFrequencyDriftUnits::m_sqrts3 ? 0.2 : 0.04,
220 to_string(_gui_covarianceClkFrequencyDriftUnit))
221 .c_str());
222
223 return changed;
224 }
225
226 private:
227 /// @brief Calculates the process noise covariance matrix with Taylor first order
228 /// @param[in] dt Time step size in [s]
229 /// @param[in] satSys Satellite system to update the keys for
230 /// @param[in] keys List of keys (bias, drift)
231 [[nodiscard]] KeyedMatrix<double, StateKeyType, StateKeyType, 2, 2>
232 calcProcessNoiseMatrixTaylor(double dt, const SatelliteSystem& satSys, const std::vector<StateKeyType>& keys) const
233 {
234 double dt2 = std::pow(dt, 2);
235 double dt3 = dt2 * dt;
236
237 KeyedMatrix<double, StateKeyType, StateKeyType, 2, 2> Q(Eigen::Matrix<double, 2, 2>::Zero(), keys, keys);
238
239 auto bias = Keys::RecvClkBias{ satSys };
240 auto drift = Keys::RecvClkDrift{ satSys };
241 Q(bias, bias) = _covarianceClkPhaseDrift * dt + _covarianceClkFrequencyDrift * dt3 / 3.0;
242 Q(bias, drift) = _covarianceClkFrequencyDrift * dt2 / 2.0;
243 Q(drift, bias) = Q(bias, drift);
244 Q(drift, drift) = _covarianceClkFrequencyDrift * dt;
245
246 return Q;
247 }
248 /// Gui selection for the Unit of the input covarianceClkPhaseDrift parameter
249 Units::CovarianceClkPhaseDriftUnits _gui_covarianceClkPhaseDriftUnit = Units::CovarianceClkPhaseDriftUnits::m2_s;
250 /// @brief GUI selection for the Standard deviation of the clock phase drift
251 double _gui_covarianceClkPhaseDrift = 0.01 /*[ m^2 / s ] */;
252
253 /// @brief Covariance of the clock phase drift [m²/s]
254 double _covarianceClkPhaseDrift = 0.01;
255
256 /// Gui selection for the Unit of the input covarianceClkFrequencyDrift parameter
257 Units::CovarianceClkFrequencyDriftUnits _gui_covarianceClkFrequencyDriftUnit = Units::CovarianceClkFrequencyDriftUnits::m2_s3;
258 /// @brief GUI selection for the Standard deviation of the clock frequency drift
259 double _gui_covarianceClkFrequencyDrift = 0.04 /* [ m^2 / s^3 ] */;
260
261 /// @brief Covariance of the clock frequency drift [m²/s³]
262 double _covarianceClkFrequencyDrift = 0.04;
263
264 /// @brief Converts the provided data into a json object
265 /// @param[out] j Json object which gets filled with the info
266 /// @param[in] data Data to convert into json
267 friend void to_json(json& j, const ReceiverClockModel& data)
268 {
269 j = {
270 { "covarianceClkPhaseDriftUnit", data._gui_covarianceClkPhaseDriftUnit },
271 { "covarianceClkPhaseDrift", data._gui_covarianceClkPhaseDrift },
272 { "covarianceClkFrequencyDriftUnit", data._gui_covarianceClkFrequencyDriftUnit },
273 { "covarianceClkFrequencyDrift", data._gui_covarianceClkFrequencyDrift },
274 };
275 }
276
277 /// @brief Converts the provided json object into the data object
278 /// @param[in] j Json object with the needed values
279 /// @param[out] data Object to fill from the json
280 friend void from_json(const json& j, ReceiverClockModel& data)
281 {
282 if (j.contains("covarianceClkPhaseDriftUnit")) { j.at("covarianceClkPhaseDriftUnit").get_to(data._gui_covarianceClkPhaseDriftUnit); }
283 if (j.contains("covarianceClkPhaseDrift")) { j.at("covarianceClkPhaseDrift").get_to(data._gui_covarianceClkPhaseDrift); }
284 if (j.contains("covarianceClkFrequencyDriftUnit")) { j.at("covarianceClkFrequencyDriftUnit").get_to(data._gui_covarianceClkFrequencyDriftUnit); }
285 if (j.contains("covarianceClkFrequencyDrift")) { j.at("covarianceClkFrequencyDrift").get_to(data._gui_covarianceClkFrequencyDrift); }
286 }
287 };
288
289 } // namespace NAV
290
291 /// @brief Stream insertion operator overload
292 /// @param[in, out] os Output stream object to stream the time into
293 /// @param[in] obj Object to print
294 /// @return Returns the output stream object in order to chain stream insertions
295 std::ostream& operator<<(std::ostream& os, const NAV::Keys::RecvClkBias& obj);
296
297 /// @brief Stream insertion operator overload
298 /// @param[in, out] os Output stream object to stream the time into
299 /// @param[in] obj Object to print
300 /// @return Returns the output stream object in order to chain stream insertions
301 std::ostream& operator<<(std::ostream& os, const NAV::Keys::RecvClkDrift& obj);
302
303 namespace std
304 {
305
306 /// @brief Hash function (needed for unordered_map)
307 template<>
308 struct hash<NAV::Keys::RecvClkBias>
309 {
310 /// @brief Hash function
311 /// @param[in] recvClkErr Receiver clock errors
312 45530 size_t operator()(const NAV::Keys::RecvClkBias& recvClkErr) const
313 {
314 45530 return std::hash<NAV::SatelliteSystem>()(recvClkErr.satSys);
315 }
316 };
317 /// @brief Hash function (needed for unordered_map)
318 template<>
319 struct hash<NAV::Keys::RecvClkDrift>
320 {
321 /// @brief Hash function
322 /// @param[in] recvClkDrift Receiver clock drifts
323 39338 size_t operator()(const NAV::Keys::RecvClkDrift& recvClkDrift) const
324 {
325 39338 return std::hash<NAV::SatelliteSystem>()(recvClkDrift.satSys);
326 }
327 };
328
329 } // namespace std
330
331 #ifndef DOXYGEN_IGNORE
332
333 /// @brief Formatter
334 template<>
335 struct fmt::formatter<NAV::Keys::RecvClkBias> : fmt::formatter<std::string>
336 {
337 /// @brief Defines how to format structs
338 /// @param[in] recvClkBias Struct to format
339 /// @param[in, out] ctx Format context
340 /// @return Output iterator
341 template<typename FormatContext>
342 auto format(const NAV::Keys::RecvClkBias& recvClkBias, FormatContext& ctx) const
343 {
344 return fmt::formatter<std::string>::format(fmt::format("RecvClkBias({})", recvClkBias.satSys), ctx);
345 }
346 };
347
348 /// @brief Formatter
349 template<>
350 struct fmt::formatter<NAV::Keys::RecvClkDrift> : fmt::formatter<std::string>
351 {
352 /// @brief Defines how to format structs
353 /// @param[in] recvClkDrift Struct to format
354 /// @param[in, out] ctx Format context
355 /// @return Output iterator
356 template<typename FormatContext>
357 auto format(const NAV::Keys::RecvClkDrift& recvClkDrift, FormatContext& ctx) const
358 {
359 return fmt::formatter<std::string>::format(fmt::format("RecvClkDrift({})", recvClkDrift.satSys), ctx);
360 }
361 };
362
363 #endif
364