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 |