INSTINCT Code Coverage Report


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