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 |