INSTINCT Code Coverage Report

Directory: src/
File: Nodes/DataProcessor/SensorCombiner/ImuFusion.cpp
Date: 2025-11-25 23:34:18
Exec Total Coverage
Lines: 305 725 42.1%
Functions: 15 22 68.2%
Branches: 276 1136 24.3%
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 "ImuFusion.hpp"
10
11 #include "util/Logger.hpp"
12
13 #include "Navigation/Transformations/Units.hpp"
14
15 #include "Navigation/INS/SensorCombiner/IRWKF/IRWKF.hpp"
16 #include "Navigation/INS/SensorCombiner/BsplineKF/BsplineKF.hpp"
17 #include "Navigation/INS/SensorCombiner/BsplineKF/QuadraticBsplines.hpp"
18 #include "NodeData/State/InsGnssLCKFSolution.hpp"
19
20 #include <imgui_internal.h>
21 #include "internal/gui/widgets/imgui_ex.hpp"
22 #include "internal/gui/widgets/InputWithUnit.hpp"
23 #include "internal/gui/widgets/EnumCombo.hpp"
24 #include "internal/gui/widgets/HelpMarker.hpp"
25 #include "internal/gui/NodeEditorApplication.hpp"
26 #include "util/Json.hpp"
27
28 #include "internal/FlowManager.hpp"
29
30 namespace NAV
31 {
32 /// @brief Write info to a json object
33 /// @param[out] j Json output
34 /// @param[in] data Object to read info from
35 ✗ static void to_json(json& j, const PinData& data) // NOLINT(misc-use-anonymous-namespace)
36 {
37 ✗ j = json{
38 // ---------------------------------------- Initialization -------------------------------------------
39 ✗ { "initAngularRateBias", data.initAngularRateBias },
40 ✗ { "initAngularRateBiasUnit", data.initAngularRateBiasUnit },
41 ✗ { "initAccelerationBias", data.initAccelerationBias },
42 ✗ { "initAccelerationBiasUnit", data.initAccelerationBiasUnit },
43 ✗ { "initCovarianceAngularRate", data.initCovarianceAngularRate },
44 ✗ { "initCovarianceAngularRateUnit", data.initCovarianceAngularRateUnit },
45 ✗ { "initCovarianceAcceleration", data.initCovarianceAcceleration },
46 ✗ { "initCovarianceAccelerationUnit", data.initCovarianceAccelerationUnit },
47 ✗ { "initCovarianceBiasAngRate", data.initCovarianceBiasAngRate },
48 ✗ { "initCovarianceBiasAngRateUnit", data.initCovarianceBiasAngRateUnit },
49 ✗ { "initCovarianceBiasAcc", data.initCovarianceBiasAcc },
50 ✗ { "initCovarianceBiasAccUnit", data.initCovarianceBiasAccUnit },
51 // ----------------------------------------- Process Noise -------------------------------------------
52 ✗ { "varBiasAccelerationNoise", data.varBiasAccelerationNoise },
53 ✗ { "varBiasAccelerationNoiseUnit", data.varBiasAccelerationNoiseUnit },
54 ✗ { "varBiasAngRateNoise", data.varBiasAngRateNoise },
55 ✗ { "varBiasAngRateNoiseUnit", data.varBiasAngRateNoiseUnit },
56 // --------------------------------------- Measurement Noise -----------------------------------------
57 ✗ { "measurementUncertaintyAngularRateUnit", data.measurementUncertaintyAngularRateUnit },
58 ✗ { "measurementUncertaintyAngularRate", data.measurementUncertaintyAngularRate },
59 ✗ { "measurementUncertaintyAccelerationUnit", data.measurementUncertaintyAccelerationUnit },
60 ✗ { "measurementUncertaintyAcceleration", data.measurementUncertaintyAcceleration },
61 ✗ };
62 ✗ }
63 /// @brief Read info from a json object
64 /// @param[in] j Json variable to read info from
65 /// @param[out] data Output object
66 9 static void from_json(const json& j, PinData& data) // NOLINT(misc-use-anonymous-namespace)
67 {
68 // ------------------------------------------ Initialization ---------------------------------------------
69
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 if (j.contains("initAngularRateBias"))
70 {
71 9 j.at("initAngularRateBias").get_to(data.initAngularRateBias);
72 }
73
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 if (j.contains("initAngularRateBiasUnit"))
74 {
75 9 j.at("initAngularRateBiasUnit").get_to(data.initAngularRateBiasUnit);
76 }
77
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 if (j.contains("initAccelerationBias"))
78 {
79 9 j.at("initAccelerationBias").get_to(data.initAccelerationBias);
80 }
81
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 if (j.contains("initAccelerationBiasUnit"))
82 {
83 9 j.at("initAccelerationBiasUnit").get_to(data.initAccelerationBiasUnit);
84 }
85
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 if (j.contains("initCovarianceAngularRate"))
86 {
87 9 j.at("initCovarianceAngularRate").get_to(data.initCovarianceAngularRate);
88 }
89
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 if (j.contains("initCovarianceAngularRateUnit"))
90 {
91 9 j.at("initCovarianceAngularRateUnit").get_to(data.initCovarianceAngularRateUnit);
92 }
93
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 if (j.contains("initCovarianceAcceleration"))
94 {
95 9 j.at("initCovarianceAcceleration").get_to(data.initCovarianceAcceleration);
96 }
97
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 if (j.contains("initCovarianceAccelerationUnit"))
98 {
99 9 j.at("initCovarianceAccelerationUnit").get_to(data.initCovarianceAccelerationUnit);
100 }
101
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 if (j.contains("initCovarianceBiasAngRate"))
102 {
103 9 j.at("initCovarianceBiasAngRate").get_to(data.initCovarianceBiasAngRate);
104 }
105
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 if (j.contains("initCovarianceBiasAngRateUnit"))
106 {
107 9 j.at("initCovarianceBiasAngRateUnit").get_to(data.initCovarianceBiasAngRateUnit);
108 }
109
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 if (j.contains("initCovarianceBiasAcc"))
110 {
111 9 j.at("initCovarianceBiasAcc").get_to(data.initCovarianceBiasAcc);
112 }
113
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 if (j.contains("initCovarianceBiasAccUnit"))
114 {
115 9 j.at("initCovarianceBiasAccUnit").get_to(data.initCovarianceBiasAccUnit);
116 }
117 // ------------------------------------------- Process Noise ---------------------------------------------
118
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 if (j.contains("varBiasAccelerationNoise"))
119 {
120 9 j.at("varBiasAccelerationNoise").get_to(data.varBiasAccelerationNoise);
121 }
122
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 if (j.contains("varBiasAccelerationNoiseUnit"))
123 {
124 9 j.at("varBiasAccelerationNoiseUnit").get_to(data.varBiasAccelerationNoiseUnit);
125 }
126
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 if (j.contains("varBiasAngRateNoise"))
127 {
128 9 j.at("varBiasAngRateNoise").get_to(data.varBiasAngRateNoise);
129 }
130
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 if (j.contains("varBiasAngRateNoiseUnit"))
131 {
132 9 j.at("varBiasAngRateNoiseUnit").get_to(data.varBiasAngRateNoiseUnit);
133 }
134 // ----------------------------------------- Measurement Noise -------------------------------------------
135
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 if (j.contains("measurementUncertaintyAngularRate"))
136 {
137 9 j.at("measurementUncertaintyAngularRate").get_to(data.measurementUncertaintyAngularRate);
138 }
139
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 if (j.contains("measurementUncertaintyAngularRateUnit"))
140 {
141 9 j.at("measurementUncertaintyAngularRateUnit").get_to(data.measurementUncertaintyAngularRateUnit);
142 }
143
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 if (j.contains("measurementUncertaintyAcceleration"))
144 {
145 9 j.at("measurementUncertaintyAcceleration").get_to(data.measurementUncertaintyAcceleration);
146 }
147
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 if (j.contains("measurementUncertaintyAccelerationUnit"))
148 {
149 9 j.at("measurementUncertaintyAccelerationUnit").get_to(data.measurementUncertaintyAccelerationUnit);
150 }
151 9 }
152
153 /// @brief Write info to a json object
154 /// @param[out] j Json output
155 /// @param[in] data Object to read info from
156 ✗ static void to_json(json& j, const PinDataIRWKF& data) // NOLINT(misc-use-anonymous-namespace)
157 {
158 ✗ j = json{
159 // ---------------------------------------- Initialization -------------------------------------------
160 ✗ { "initAngularRate", data.initAngularRate },
161 ✗ { "initAngularRateUnit", data.initAngularRateUnit },
162 ✗ { "initAcceleration", data.initAcceleration },
163 ✗ { "initAccelerationUnit", data.initAccelerationUnit },
164 ✗ { "initAngularAcc", data.initAngularAcc },
165 ✗ { "initAngularAccUnit", data.initAngularAccUnit },
166 ✗ { "initJerk", data.initJerk },
167 ✗ { "initJerkUnit", data.initJerkUnit },
168 ✗ { "initCovarianceAngularAcc", data.initCovarianceAngularAcc },
169 ✗ { "initCovarianceAngularAccUnit", data.initCovarianceAngularAccUnit },
170 ✗ { "initCovarianceJerk", data.initCovarianceJerk },
171 ✗ { "initCovarianceJerkUnit", data.initCovarianceJerkUnit },
172
173 // ----------------------------------------- Process Noise -------------------------------------------
174 ✗ { "varAngularAccNoise", data.varAngularAccNoise },
175 ✗ { "varAngularAccNoiseUnit", data.varAngularAccNoiseUnit },
176 ✗ { "varJerkNoise", data.varJerkNoise },
177 ✗ { "varJerkNoiseUnit", data.varJerkNoiseUnit },
178 ✗ };
179 ✗ }
180 /// @brief Read info from a json object
181 /// @param[in] j Json variable to read info from
182 /// @param[out] data Output object
183 2 static void from_json(const json& j, PinDataIRWKF& data) // NOLINT(misc-use-anonymous-namespace)
184 {
185 // ------------------------------------------ Initialization ---------------------------------------------
186
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("initAngularRate"))
187 {
188 2 j.at("initAngularRate").get_to(data.initAngularRate);
189 }
190
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("initAngularRateUnit"))
191 {
192 2 j.at("initAngularRateUnit").get_to(data.initAngularRateUnit);
193 }
194
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("initAcceleration"))
195 {
196 2 j.at("initAcceleration").get_to(data.initAcceleration);
197 }
198
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("initAccelerationUnit"))
199 {
200 2 j.at("initAccelerationUnit").get_to(data.initAccelerationUnit);
201 }
202
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("initAngularAcc"))
203 {
204 2 j.at("initAngularAcc").get_to(data.initAngularAcc);
205 }
206
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("initAngularAccUnit"))
207 {
208 2 j.at("initAngularAccUnit").get_to(data.initAngularAccUnit);
209 }
210
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("initJerk"))
211 {
212 2 j.at("initJerk").get_to(data.initJerk);
213 }
214
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("initJerkUnit"))
215 {
216 2 j.at("initJerkUnit").get_to(data.initJerkUnit);
217 }
218
219
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("initCovarianceAngularAcc"))
220 {
221 2 j.at("initCovarianceAngularAcc").get_to(data.initCovarianceAngularAcc);
222 }
223
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("initCovarianceAngularAccUnit"))
224 {
225 2 j.at("initCovarianceAngularAccUnit").get_to(data.initCovarianceAngularAccUnit);
226 }
227
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("initCovarianceJerk"))
228 {
229 2 j.at("initCovarianceJerk").get_to(data.initCovarianceJerk);
230 }
231
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("initCovarianceJerkUnit"))
232 {
233 2 j.at("initCovarianceJerkUnit").get_to(data.initCovarianceJerkUnit);
234 }
235
236 // ------------------------------------------- Process Noise ---------------------------------------------
237
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("varAngularAccNoise"))
238 {
239 2 j.at("varAngularAccNoise").get_to(data.varAngularAccNoise);
240 }
241
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("varAngularAccNoiseUnit"))
242 {
243 2 j.at("varAngularAccNoiseUnit").get_to(data.varAngularAccNoiseUnit);
244 }
245
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("varJerkNoise"))
246 {
247 2 j.at("varJerkNoise").get_to(data.varJerkNoise);
248 }
249
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("varJerkNoiseUnit"))
250 {
251 2 j.at("varJerkNoiseUnit").get_to(data.varJerkNoiseUnit);
252 }
253 2 }
254
255 /// @brief Write info to a json object
256 /// @param[out] j Json output
257 /// @param[in] data Object to read info from
258 ✗ static void to_json(json& j, const PinDataBsplineKF& data) // NOLINT(misc-use-anonymous-namespace)
259 {
260 ✗ j = json{
261 // ---------------------------------------- Initialization -------------------------------------------
262 ✗ { "initAngularRate", data.initCoeffsAngRate },
263 ✗ { "initCoeffsAngularRateUnit", data.initCoeffsAngularRateUnit },
264 ✗ { "initCoeffsAccel", data.initCoeffsAccel },
265 ✗ { "initCoeffsAccelUnit", data.initCoeffsAccelUnit },
266 ✗ { "initCovarianceCoeffsAngRate", data.initCovarianceCoeffsAngRate },
267 ✗ { "initCovarianceCoeffsAngRateUnit", data.initCovarianceCoeffsAngRateUnit },
268 ✗ { "initCovarianceCoeffsAccel", data.initCovarianceCoeffsAccel },
269 ✗ { "initCovarianceCoeffsAccelUnit", data.initCovarianceCoeffsAccelUnit },
270
271 // ----------------------------------------- Process Noise -------------------------------------------
272 ✗ { "varCoeffsAngRateNoise", data.varCoeffsAngRateNoise },
273 ✗ { "varCoeffsAngRateUnit", data.varCoeffsAngRateUnit },
274 ✗ { "varCoeffsAccelNoise", data.varCoeffsAccelNoise },
275 ✗ { "varCoeffsAccelUnit", data.varCoeffsAccelUnit },
276 ✗ };
277 ✗ }
278 /// @brief Read info from a json object
279 /// @param[in] j Json variable to read info from
280 /// @param[out] data Output object
281 2 static void from_json(const json& j, PinDataBsplineKF& data) // NOLINT(misc-use-anonymous-namespace)
282 {
283 // ------------------------------------------ Initialization ---------------------------------------------
284
285
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 if (j.contains("initCoeffsAngRate"))
286 {
287 ✗ j.at("initCoeffsAngRate").get_to(data.initCoeffsAngRate);
288 }
289
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("initCoeffsAngularRateUnit"))
290 {
291 2 j.at("initCoeffsAngularRateUnit").get_to(data.initCoeffsAngularRateUnit);
292 }
293
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("initCoeffsAccel"))
294 {
295 2 j.at("initCoeffsAccel").get_to(data.initCoeffsAccel);
296 }
297
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("initCoeffsAccelUnit"))
298 {
299 2 j.at("initCoeffsAccelUnit").get_to(data.initCoeffsAccelUnit);
300 }
301
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("initCovarianceCoeffsAngRate"))
302 {
303 2 j.at("initCovarianceCoeffsAngRate").get_to(data.initCovarianceCoeffsAngRate);
304 }
305
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("initCovarianceCoeffsAngRateUnit"))
306 {
307 2 j.at("initCovarianceCoeffsAngRateUnit").get_to(data.initCovarianceCoeffsAngRateUnit);
308 }
309
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("initCovarianceCoeffsAccel"))
310 {
311 2 j.at("initCovarianceCoeffsAccel").get_to(data.initCovarianceCoeffsAccel);
312 }
313
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("initCovarianceCoeffsAccelUnit"))
314 {
315 2 j.at("initCovarianceCoeffsAccelUnit").get_to(data.initCovarianceCoeffsAccelUnit);
316 }
317
318 // ------------------------------------------- Process Noise ---------------------------------------------
319
320
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("varCoeffsAngRateNoise"))
321 {
322 2 j.at("varCoeffsAngRateNoise").get_to(data.varCoeffsAngRateNoise);
323 }
324
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("varCoeffsAngRateUnit"))
325 {
326 2 j.at("varCoeffsAngRateUnit").get_to(data.varCoeffsAngRateUnit);
327 }
328
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("varCoeffsAccelNoise"))
329 {
330 2 j.at("varCoeffsAccelNoise").get_to(data.varCoeffsAccelNoise);
331 }
332
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("varCoeffsAccelUnit"))
333 {
334 2 j.at("varCoeffsAccelUnit").get_to(data.varCoeffsAccelUnit);
335 }
336 2 }
337
338 } // namespace NAV
339
340 116 NAV::ImuFusion::ImuFusion()
341
12/24
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 116 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 116 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 116 times.
✗ Branch 13 not taken.
✓ Branch 19 taken 116 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 116 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 116 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 116 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 116 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 116 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 116 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 116 times.
✗ Branch 41 not taken.
 116 : Imu(typeStatic())
342 {
343 LOG_TRACE("{}: called", name);
344
345 116 _hasConfig = true;
346 116 _guiConfigDefaultWindowSize = { 991, 1059 };
347
348
4/8
✓ Branch 2 taken 116 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 116 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 116 times.
✓ Branch 10 taken 116 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 464 CreateOutputPin("Combined ImuObs", Pin::Type::Flow, { NAV::ImuObs::type() });
349
1/2
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
 116 updateNumberOfInputPins();
350 232 }
351
352 236 NAV::ImuFusion::~ImuFusion()
353 {
354 LOG_TRACE("{}: called", nameId());
355 236 }
356
357 230 std::string NAV::ImuFusion::typeStatic()
358 {
359
1/2
✓ Branch 1 taken 230 times.
✗ Branch 2 not taken.
 460 return "ImuFusion";
360 }
361
362 ✗ std::string NAV::ImuFusion::type() const
363 {
364 ✗ return typeStatic();
365 }
366
367 114 std::string NAV::ImuFusion::category()
368 {
369
1/2
✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
 228 return "Data Processor";
370 }
371
372 ✗ void NAV::ImuFusion::guiConfig()
373 {
374 ✗ constexpr float configWidth = 380.0F;
375 ✗ constexpr float unitWidth = 150.0F;
376
377 ✗ if (ImGui::BeginTable(fmt::format("Pin Settings##{}", size_t(id)).c_str(), inputPins.size() > 1 ? 2 : 1,
378 ✗ ImGuiTableFlags_Borders | ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_NoHostExtendX, ImVec2(0.0F, 0.0F)))
379 {
380 ✗ ImGui::TableSetupColumn("Pin");
381 ✗ if (inputPins.size() > 2)
382 {
383 ✗ ImGui::TableSetupColumn("");
384 }
385 ✗ ImGui::TableHeadersRow();
386
387 ✗ for (size_t pinIndex = 0; pinIndex < _pinData.size(); ++pinIndex)
388 {
389 ✗ ImGui::TableNextRow();
390 ✗ ImGui::TableNextColumn(); // Pin
391
392 ✗ ImGui::TextUnformatted(fmt::format("{}", inputPins.at(pinIndex).name).c_str());
393
394 ✗ if (inputPins.size() > 2) // Minimum # of pins for the fusion to make sense is two
395 {
396 ✗ ImGui::TableNextColumn(); // Delete
397 ✗ if (!(pinIndex == 0)) // Don't delete Pin 1, it's the reference for all other sensor (biases) that follow
398 {
399 ✗ if (ImGui::Button(fmt::format("x##{} - {}", size_t(id), pinIndex).c_str()))
400 {
401 ✗ DeleteInputPin(pinIndex);
402 ✗ DeleteOutputPin(pinIndex);
403 ✗ _pinData.erase(_pinData.begin() + static_cast<int64_t>(pinIndex - 1));
404 ✗ --_nInputPins;
405 ✗ flow::ApplyChanges();
406 ✗ updateNumberOfInputPins();
407 }
408 ✗ if (ImGui::IsItemHovered())
409 {
410 ✗ ImGui::SetTooltip("Delete the pin");
411 }
412 }
413 }
414 }
415
416 ✗ ImGui::TableNextRow();
417 ✗ ImGui::TableNextColumn(); // Pin
418 ✗ if (ImGui::Button(fmt::format("Add Pin##{}", size_t(id)).c_str()))
419 {
420 ✗ ++_nInputPins;
421 ✗ LOG_DEBUG("{}: # Input Pins changed to {}", nameId(), _nInputPins);
422 ✗ flow::ApplyChanges();
423 ✗ updateNumberOfInputPins();
424 }
425
426 ✗ ImGui::EndTable();
427 }
428
429 ✗ float columnWidth = 130.0F * gui::NodeEditorApplication::windowFontRatio();
430
431 ✗ ImGui::Separator();
432
433 // #######################################################################################################
434 // KF config
435 // #######################################################################################################
436 ✗ ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
437
438 ✗ ImGui::SetNextItemWidth(columnWidth);
439 ✗ if (gui::widgets::EnumCombo(fmt::format("IMU fusion type##{}", size_t(id)).c_str(), _imuFusionType))
440 {
441 ✗ LOG_DEBUG("{}: imuFusionType changed to {}", nameId(), fmt::underlying(_imuFusionType));
442
443 ✗ flow::ApplyChanges();
444 ✗ doDeinitialize();
445 }
446 ✗ ImGui::SameLine();
447 ✗ gui::widgets::HelpMarker("IRWKF: Integrated-Random-Walk Kalman-Filter (estimates angular acceleration and jerk).\nB-spline KF: Kalman-Filter that estimates three equally spaced quadratic B-splines for angular rate and acceleration, respectively.");
448
449 ✗ ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
450
451 ✗ ImGui::SetNextItemWidth(columnWidth);
452 ✗ if (ImGui::InputDoubleL(fmt::format("Highest IMU sample rate in [Hz]##{}", size_t(id)).c_str(), &_imuFrequency, 1e-3, 1e4, 0.0, 0.0, "%.0f"))
453 {
454 ✗ LOG_DEBUG("{}: imuFrequency changed to {}", nameId(), _imuFrequency);
455 ✗ flow::ApplyChanges();
456 }
457 ✗ ImGui::SameLine();
458 ✗ gui::widgets::HelpMarker("The inverse of this rate is used as the initial 'dt' for the Kalman Filter Prediction (Phi and Q).");
459
460 ✗ if (_imuFusionType == ImuFusionType::Bspline)
461 {
462 ✗ ImGui::SetNextItemWidth(columnWidth);
463 ✗ if (ImGui::InputDoubleL(fmt::format("Spacing between the quadratic B-splines in [s]##{}", size_t(id)).c_str(), &_splineSpacing, 1e-3, 1.0, 0.0, 0.0, "%.3f"))
464 {
465 ✗ LOG_DEBUG("{}: splineSpacing changed to {}", nameId(), _splineSpacing);
466 ✗ flow::ApplyChanges();
467 }
468 ✗ ImGui::SameLine();
469 ✗ gui::widgets::HelpMarker("Time difference between each quadratic B-spline, maximum: 1.0 second");
470 }
471
472 ✗ if (ImGui::Checkbox(fmt::format("Rank check for Kalman filter matrices##{}", size_t(id)).c_str(), &_checkKalmanMatricesRanks))
473 {
474 ✗ LOG_DEBUG("{}: checkKalmanMatricesRanks {}", nameId(), _checkKalmanMatricesRanks);
475 ✗ flow::ApplyChanges();
476 }
477 ✗ ImGui::SameLine();
478 ✗ gui::widgets::HelpMarker("Computationally intensive - only recommended for debugging.");
479
480 ✗ if (_imuFusionType != ImuFusionType::IRWKF)
481 {
482 ✗ ImGui::BeginDisabled();
483 }
484 ✗ if (ImGui::Checkbox(fmt::format("Auto-initialize Kalman filter##{}", size_t(id)).c_str(), &_autoInitKF))
485 {
486 LOG_DATA("{}: auto-initialize KF: {}", nameId(), _autoInitKF);
487 ✗ flow::ApplyChanges();
488 }
489 ✗ if (_imuFusionType != ImuFusionType::IRWKF)
490 {
491 ✗ if (_autoInitKF)
492 {
493 ✗ _autoInitKF = false;
494 ✗ LOG_INFO("{}: Auto-initialization for KF turned off. This is currently only available for the IRWKF.", nameId());
495 }
496 ✗ ImGui::EndDisabled();
497 }
498 ✗ ImGui::SameLine();
499 ✗ gui::widgets::HelpMarker("Initializes the KF by averaging the data over a specified time frame. Currently only available for the IRWKF fusion type.");
500 ✗ if (ImGui::Checkbox(fmt::format("Characteristics of the multiple IMUs are identical##{}", size_t(id)).c_str(), &_imuCharacteristicsIdentical))
501 {
502 LOG_DATA("{}: imuCharacteristicsIdentical: {}", nameId(), _imuCharacteristicsIdentical);
503 ✗ flow::ApplyChanges();
504 }
505 ✗ ImGui::SameLine();
506 ✗ gui::widgets::HelpMarker("GUI input cells can be reduced considerably.");
507 ✗ if (ImGui::Checkbox(fmt::format("Biases of the multiple IMUs are identical##{}", size_t(id)).c_str(), &_imuBiasesIdentical))
508 {
509 LOG_DATA("{}: imuBiasesIdentical: {}", nameId(), _imuBiasesIdentical);
510 ✗ flow::ApplyChanges();
511 }
512 ✗ ImGui::SameLine();
513 ✗ gui::widgets::HelpMarker("GUI input cells can be reduced considerably.");
514
515 ✗ ImGui::Separator();
516
517 // #######################################################################################################
518 // KF initialization
519 // #######################################################################################################
520
521 ✗ if (_autoInitKF)
522 {
523 ✗ ImGui::Text("Kalman Filter initialization (auto-init)");
524
525 ✗ ImGui::SetNextItemWidth(columnWidth);
526 ✗ if (ImGui::InputDoubleL(fmt::format("Averaging time in [s]##{}", size_t(id)).c_str(), &_averageEndTime, 1e-3, 1e4, 0.0, 0.0, "%.0f"))
527 {
528 ✗ LOG_DEBUG("{}: averageEndTime changed to {}", nameId(), _averageEndTime);
529 ✗ flow::ApplyChanges();
530 }
531 ✗ ImGui::SameLine();
532 ✗ gui::widgets::HelpMarker("Determines how long the data is averaged before the KF is auto-initialized");
533
534 ✗ if (ImGui::Checkbox(fmt::format("Initialize Jerk variance to acceleration variance and angular acceleration variance to angular rate variance##{}", size_t(id)).c_str(), &_initJerkAngAcc))
535 {
536 LOG_DATA("{}: initJerkAngAcc: {}", nameId(), _initJerkAngAcc);
537 ✗ flow::ApplyChanges();
538 }
539 ✗ ImGui::SameLine();
540 ✗ gui::widgets::HelpMarker("Otherwise zero");
541 }
542 else
543 {
544 ✗ ImGui::Text("Kalman Filter initialization (manual init)");
545
546 // ---------------------------------------- State vector x0 -------------------------------------------
547 ✗ ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
548 ✗ if (ImGui::TreeNode(fmt::format("x - State vector##{}", size_t(id)).c_str()))
549 {
550 ✗ if (_imuFusionType == ImuFusionType::IRWKF)
551 {
552 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Angular rate##{}", size_t(id)).c_str(),
553 ✗ configWidth, unitWidth, _pinDataIRWKF.initAngularRate.data(), _pinDataIRWKF.initAngularRateUnit, "deg/s\0"
554 "rad/s\0\0",
555 "%.2e", ImGuiInputTextFlags_CharsScientific))
556 {
557 LOG_DATA("{}: initAngularRate changed to {}", nameId(), _pinDataIRWKF.initAngularRate);
558 LOG_DATA("{}: AngularRateUnit changed to {}", nameId(), fmt::underlying(_pinDataIRWKF.initAngularRateUnit));
559 ✗ flow::ApplyChanges();
560 }
561
562 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Acceleration##{}", size_t(id)).c_str(),
563 ✗ configWidth, unitWidth, _pinDataIRWKF.initAcceleration.data(), _pinDataIRWKF.initAccelerationUnit, "m/s²\0\0", "%.2e", ImGuiInputTextFlags_CharsScientific))
564 {
565 LOG_DATA("{}: initAcceleration changed to {}", nameId(), _pinDataIRWKF.initAcceleration);
566 LOG_DATA("{}: initAccelerationUnit changed to {}", nameId(), fmt::underlying(_pinDataIRWKF.initAccelerationUnit));
567 ✗ flow::ApplyChanges();
568 }
569
570 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Angular Acceleration##{}", size_t(id)).c_str(),
571 ✗ configWidth, unitWidth, _pinDataIRWKF.initAngularAcc.data(), _pinDataIRWKF.initAngularAccUnit, "deg/s²\0"
572 "rad/s^2\0\0",
573 "%.2e", ImGuiInputTextFlags_CharsScientific))
574 {
575 LOG_DATA("{}: initAngularAcc changed to {}", nameId(), _pinDataIRWKF.initAngularAcc);
576 LOG_DATA("{}: initAngularAccUnit changed to {}", nameId(), fmt::underlying(_pinDataIRWKF.initAngularAccUnit));
577 ✗ flow::ApplyChanges();
578 }
579
580 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Jerk##{}", size_t(id)).c_str(),
581 ✗ configWidth, unitWidth, _pinDataIRWKF.initJerk.data(), _pinDataIRWKF.initJerkUnit, "m/s³\0\0",
582 "%.2e", ImGuiInputTextFlags_CharsScientific))
583 {
584 LOG_DATA("{}: initJerk changed to {}", nameId(), _pinDataIRWKF.initJerk);
585 LOG_DATA("{}: PinData::JerkVarianceUnit changed to {}", nameId(), fmt::underlying(_pinDataIRWKF.initJerkUnit));
586 ✗ flow::ApplyChanges();
587 }
588 }
589 else // (_imuFusionType == ImuFusionType::Bspline)
590 {
591 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("B-spline coefficients for the angular rate##{}", size_t(id)).c_str(),
592 ✗ configWidth, unitWidth, _initCoeffsAngRateTemp.data(), _pinDataBsplineKF.initCoeffsAngularRateUnit, "deg/s\0"
593 "rad/s\0\0",
594 "%.2e", ImGuiInputTextFlags_CharsScientific))
595 {
596 LOG_DATA("{}: initCoeffsAngularRateUnit changed to {}", nameId(), fmt::underlying(_pinDataBsplineKF.initCoeffsAngularRateUnit));
597 ✗ flow::ApplyChanges();
598 }
599 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("B-spline coefficients for the acceleration##{}", size_t(id)).c_str(),
600 ✗ configWidth, unitWidth, _initCoeffsAccelTemp.data(), _pinDataBsplineKF.initCoeffsAccelUnit, "m/s²\0\0",
601 "%.2e", ImGuiInputTextFlags_CharsScientific))
602 {
603 LOG_DATA("{}: initCoeffsAccelUnit changed to {}", nameId(), fmt::underlying(_pinDataBsplineKF.initCoeffsAccelUnit));
604 ✗ flow::ApplyChanges();
605 }
606 ✗ for (uint8_t i = 0; i < _numBsplines; i += 3)
607 {
608 ✗ _pinDataBsplineKF.initCoeffsAngRate.block<3, 1>(i, 0) = _initCoeffsAngRateTemp;
609 ✗ _pinDataBsplineKF.initCoeffsAccel.block<3, 1>(i, 0) = _initCoeffsAccelTemp;
610 }
611 LOG_DATA("{}: initCoeffsAngRate changed to {}", nameId(), _pinDataBsplineKF.initCoeffsAngRate);
612 LOG_DATA("{}: initCoeffsAccel changed to {}", nameId(), _pinDataBsplineKF.initCoeffsAccel);
613 }
614
615 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Angular rate bias of sensor {}##{}", 2, size_t(id)).c_str(),
616 ✗ configWidth, unitWidth, _pinData[1].initAngularRateBias.data(), _pinData[1].initAngularRateBiasUnit, "deg/s\0rad/s\0\0",
617 "%.2e", ImGuiInputTextFlags_CharsScientific))
618 {
619 ✗ flow::ApplyChanges();
620 }
621
622 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Acceleration bias of sensor {}##{}", 2, size_t(id)).c_str(),
623 ✗ configWidth, unitWidth, _pinData[1].initAccelerationBias.data(), _pinData[1].initAccelerationBiasUnit, "m/s^2\0\0",
624 "%.2e", ImGuiInputTextFlags_CharsScientific))
625 {
626 ✗ flow::ApplyChanges();
627 }
628 ✗ if (!_imuBiasesIdentical)
629 {
630 ✗ for (size_t pinIndex = 2; pinIndex < _nInputPins; ++pinIndex)
631 {
632 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Angular rate bias of sensor {}##{}", pinIndex + 1, size_t(id)).c_str(),
633 ✗ configWidth, unitWidth, _pinData[pinIndex].initAngularRateBias.data(), _pinData[pinIndex].initAngularRateBiasUnit, "deg/s\0rad/s\0\0",
634 "%.2e", ImGuiInputTextFlags_CharsScientific))
635 {
636 ✗ flow::ApplyChanges();
637 }
638
639 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Acceleration bias of sensor {}##{}", pinIndex + 1, size_t(id)).c_str(),
640 ✗ configWidth, unitWidth, _pinData[pinIndex].initAccelerationBias.data(), _pinData[pinIndex].initAccelerationBiasUnit, "m/s^2\0\0",
641 "%.2e", ImGuiInputTextFlags_CharsScientific))
642 {
643 ✗ flow::ApplyChanges();
644 }
645 }
646 }
647
648 ✗ ImGui::TreePop();
649 }
650
651 // ----------------------------------- Error covariance matrix P0 -------------------------------------
652 ✗ ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
653 ✗ if (ImGui::TreeNode(fmt::format("P - Error covariance matrix##{}", size_t(id)).c_str()))
654 {
655 ✗ if (_imuFusionType == ImuFusionType::IRWKF)
656 {
657 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Angular rate covariance ({})##{}",
658 ✗ _pinData[0].initCovarianceAngularRateUnit == PinData::AngRateVarianceUnit::rad2_s2
659 ✗ || _pinData[0].initCovarianceAngularRateUnit == PinData::AngRateVarianceUnit::deg2_s2
660 ✗ ? "Variance σ²"
661 : "Standard deviation σ",
662 ✗ size_t(id))
663 .c_str(),
664 ✗ configWidth, unitWidth, _pinData[0].initCovarianceAngularRate.data(), _pinData[0].initCovarianceAngularRateUnit, "(rad/s)²\0"
665 "rad/s\0"
666 "(deg/s)²\0"
667 "deg/s\0\0",
668 "%.2e", ImGuiInputTextFlags_CharsScientific))
669 {
670 LOG_DATA("{}: initCovarianceAngularRate changed to {}", nameId(), _pinData[0].initCovarianceAngularRate);
671 LOG_DATA("{}: AngRateVarianceUnit changed to {}", nameId(), fmt::underlying(_pinData[0].initCovarianceAngularRateUnit));
672 ✗ flow::ApplyChanges();
673 }
674
675 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Angular acceleration covariance ({})##{}",
676 ✗ _pinDataIRWKF.initCovarianceAngularAccUnit == PinDataIRWKF::AngularAccVarianceUnit::rad2_s4
677 ✗ || _pinDataIRWKF.initCovarianceAngularAccUnit == PinDataIRWKF::AngularAccVarianceUnit::deg2_s4
678 ✗ ? "Variance σ²"
679 : "Standard deviation σ",
680 ✗ size_t(id))
681 .c_str(),
682 ✗ configWidth, unitWidth, _pinDataIRWKF.initCovarianceAngularAcc.data(), _pinDataIRWKF.initCovarianceAngularAccUnit, "(rad^2)/(s^4)\0"
683 "rad/s^2\0"
684 "(deg^2)/(s^4)\0"
685 "deg/s^2\0\0",
686 "%.2e", ImGuiInputTextFlags_CharsScientific))
687 {
688 LOG_DATA("{}: initCovarianceAngularAcc changed to {}", nameId(), _pinDataIRWKF.initCovarianceAngularAcc);
689 LOG_DATA("{}: PinData::AngularAccVarianceUnit changed to {}", nameId(), fmt::underlying(_pinDataIRWKF.initCovarianceAngularAccUnit));
690 ✗ flow::ApplyChanges();
691 }
692
693 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Acceleration covariance ({})##{}",
694 ✗ _pinData[0].initCovarianceAccelerationUnit == PinData::AccelerationVarianceUnit::m2_s4
695 ✗ ? "Variance σ²"
696 : "Standard deviation σ",
697 ✗ size_t(id))
698 .c_str(),
699 ✗ configWidth, unitWidth, _pinData[0].initCovarianceAcceleration.data(), _pinData[0].initCovarianceAccelerationUnit, "(m^2)/(s^4)\0"
700 "m/s^2\0\0",
701 "%.2e", ImGuiInputTextFlags_CharsScientific))
702 {
703 LOG_DATA("{}: initCovarianceAcceleration changed to {}", nameId(), _pinData[0].initCovarianceAcceleration);
704 LOG_DATA("{}: PinData::AccelerationVarianceUnit changed to {}", nameId(), fmt::underlying(_pinData[0].initCovarianceAccelerationUnit));
705 ✗ flow::ApplyChanges();
706 }
707
708 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Jerk covariance ({})##{}",
709 ✗ _pinDataIRWKF.initCovarianceJerkUnit == PinDataIRWKF::JerkVarianceUnit::m2_s6
710 ✗ ? "Variance σ²"
711 : "Standard deviation σ",
712 ✗ size_t(id))
713 .c_str(),
714 ✗ configWidth, unitWidth, _pinDataIRWKF.initCovarianceJerk.data(), _pinDataIRWKF.initCovarianceJerkUnit, "(m^2)/(s^6)\0"
715 "m/s^3\0\0",
716 "%.2e", ImGuiInputTextFlags_CharsScientific))
717 {
718 LOG_DATA("{}: initCovarianceJerk changed to {}", nameId(), _pinDataIRWKF.initCovarianceJerk);
719 LOG_DATA("{}: PinData::JerkVarianceUnit changed to {}", nameId(), fmt::underlying(_pinDataIRWKF.initCovarianceJerkUnit));
720 ✗ flow::ApplyChanges();
721 }
722 }
723 else // (_imuFusionType == ImuFusionType::Bspline)
724 {
725 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Covariance of the B-spline coefficients of the angular rate ({})##{}",
726 ✗ _pinDataBsplineKF.initCovarianceCoeffsAngRateUnit == PinData::AngRateVarianceUnit::rad2_s2
727 ✗ || _pinDataBsplineKF.initCovarianceCoeffsAngRateUnit == PinData::AngRateVarianceUnit::deg2_s2
728 ✗ ? "Variance σ²"
729 : "Standard deviation σ",
730 ✗ size_t(id))
731 .c_str(),
732 ✗ configWidth, unitWidth, _initCovarianceCoeffsAngRateTemp.data(), _pinDataBsplineKF.initCovarianceCoeffsAngRateUnit, "(rad/s)²\0"
733 "rad/s\0"
734 "(deg/s)²\0"
735 "deg/s\0\0",
736 "%.2e", ImGuiInputTextFlags_CharsScientific))
737 {
738 LOG_DATA("{}: initCovarianceCoeffsAngRateUnit changed to {}", nameId(), fmt::underlying(_pinDataBsplineKF.initCovarianceCoeffsAngRateUnit));
739 ✗ flow::ApplyChanges();
740 }
741 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Covariance of the B-spline coefficients of the acceleration ({})##{}",
742 ✗ _pinDataBsplineKF.initCovarianceCoeffsAccelUnit == PinData::AccelerationVarianceUnit::m2_s4
743 ✗ ? "Variance σ²"
744 : "Standard deviation σ",
745 ✗ size_t(id))
746 .c_str(),
747 ✗ configWidth, unitWidth, _initCovarianceCoeffsAccelTemp.data(), _pinDataBsplineKF.initCovarianceCoeffsAccelUnit, "(m^2)/(s^4)\0"
748 "m/s^2\0\0",
749 "%.2e", ImGuiInputTextFlags_CharsScientific))
750 {
751 LOG_DATA("{}: initCovarianceCoeffsAccelUnit changed to {}", nameId(), fmt::underlying(_pinDataBsplineKF.initCovarianceCoeffsAccelUnit));
752 ✗ flow::ApplyChanges();
753 }
754 ✗ for (uint8_t i = 0; i < _numBsplines; i += 3)
755 {
756 ✗ _pinDataBsplineKF.initCovarianceCoeffsAngRate.block<3, 1>(i, 0) = _initCovarianceCoeffsAngRateTemp;
757 ✗ _pinDataBsplineKF.initCovarianceCoeffsAccel.block<3, 1>(i, 0) = _initCovarianceCoeffsAccelTemp;
758 }
759 LOG_DATA("{}: initCovarianceCoeffsAngRate changed to {}", nameId(), _pinDataBsplineKF.initCovarianceCoeffsAngRate);
760 LOG_DATA("{}: initCovarianceCoeffsAccel changed to {}", nameId(), _pinDataBsplineKF.initCovarianceCoeffsAccel);
761 }
762
763 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Angular rate bias covariance of sensor {} ({})##{}", 2,
764 ✗ _pinData[1].initCovarianceBiasAngRateUnit == PinData::AngRateVarianceUnit::rad2_s2
765 ✗ || _pinData[1].initCovarianceBiasAngRateUnit == PinData::AngRateVarianceUnit::deg2_s2
766 ✗ ? "Variance σ²"
767 : "Standard deviation σ",
768 ✗ size_t(id))
769 .c_str(),
770 ✗ configWidth, unitWidth, _pinData[1].initCovarianceBiasAngRate.data(), _pinData[1].initCovarianceBiasAngRateUnit, "(rad^2)/(s^2)\0"
771 "rad/s\0"
772 "(deg^2)/(s^2)\0"
773 "deg/s\0\0",
774 "%.2e", ImGuiInputTextFlags_CharsScientific))
775 {
776 LOG_DATA("{}: initCovarianceBiasAngRate changed to {}", nameId(), _pinData[1].initCovarianceBiasAngRate);
777 LOG_DATA("{}: PinData::AngRateVarianceUnit changed to {}", nameId(), fmt::underlying(_pinData[1].initCovarianceBiasAngRateUnit));
778 ✗ flow::ApplyChanges();
779 }
780
781 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Acceleration bias covariance of sensor {} ({})##{}", 2,
782 ✗ _pinData[1].initCovarianceBiasAccUnit == PinData::AccelerationVarianceUnit::m2_s4
783 ✗ ? "Variance σ²"
784 : "Standard deviation σ",
785 ✗ size_t(id))
786 .c_str(),
787 ✗ configWidth, unitWidth, _pinData[1].initCovarianceBiasAcc.data(), _pinData[1].initCovarianceBiasAccUnit, "(m^2)/(s^4)\0"
788 "m/s^2\0\0",
789 "%.2e", ImGuiInputTextFlags_CharsScientific))
790 {
791 LOG_DATA("{}: initCovarianceBiasAcc changed to {}", nameId(), _pinData[1].initCovarianceBiasAcc);
792 LOG_DATA("{}: PinData::AccelerationVarianceUnit changed to {}", nameId(), fmt::underlying(_pinData[1].initCovarianceBiasAccUnit));
793 ✗ flow::ApplyChanges();
794 }
795 ✗ if (!_imuCharacteristicsIdentical)
796 {
797 ✗ for (size_t pinIndex = 2; pinIndex < _nInputPins; ++pinIndex)
798 {
799 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Angular rate bias covariance of sensor {} ({})##{}", pinIndex + 1,
800 ✗ _pinData[pinIndex].initCovarianceBiasAngRateUnit == PinData::AngRateVarianceUnit::rad2_s2
801 ✗ || _pinData[pinIndex].initCovarianceBiasAngRateUnit == PinData::AngRateVarianceUnit::deg2_s2
802 ✗ ? "Variance σ²"
803 : "Standard deviation σ",
804 ✗ size_t(id))
805 .c_str(),
806 ✗ configWidth, unitWidth, _pinData[pinIndex].initCovarianceBiasAngRate.data(), _pinData[pinIndex].initCovarianceBiasAngRateUnit, "(rad^2)/(s^2)\0"
807 "rad/s\0"
808 "(deg^2)/(s^2)\0"
809 "deg/s\0\0",
810 "%.2e", ImGuiInputTextFlags_CharsScientific))
811 {
812 LOG_DATA("{}: initCovarianceBiasAngRate changed to {}", nameId(), _pinData[pinIndex].initCovarianceBiasAngRate);
813 LOG_DATA("{}: PinData::AngRateVarianceUnit changed to {}", nameId(), fmt::underlying(_pinData[pinIndex].initCovarianceBiasAngRateUnit));
814 ✗ flow::ApplyChanges();
815 }
816
817 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Acceleration bias covariance of sensor {} ({})##{}", pinIndex + 1,
818 ✗ _pinData[pinIndex].initCovarianceBiasAccUnit == PinData::AccelerationVarianceUnit::m2_s4
819 ✗ ? "Variance σ²"
820 : "Standard deviation σ",
821 ✗ size_t(id))
822 .c_str(),
823 ✗ configWidth, unitWidth, _pinData[pinIndex].initCovarianceBiasAcc.data(), _pinData[pinIndex].initCovarianceBiasAccUnit, "(m^2)/(s^4)\0"
824 "m/s^2\0\0",
825 "%.2e", ImGuiInputTextFlags_CharsScientific))
826 {
827 LOG_DATA("{}: initCovarianceBiasAcc changed to {}", nameId(), _pinData[pinIndex].initCovarianceBiasAcc);
828 LOG_DATA("{}: PinData::AccelerationVarianceUnit changed to {}", nameId(), fmt::underlying(_pinData[pinIndex].initCovarianceBiasAccUnit));
829 ✗ flow::ApplyChanges();
830 }
831 }
832 }
833
834 ✗ ImGui::TreePop();
835 }
836 }
837
838 ✗ ImGui::Separator();
839
840 // #######################################################################################################
841 // KF noise setting
842 // #######################################################################################################
843 ✗ ImGui::Text("Kalman Filter noise setting");
844
845 // -------------------------------------- Process noise matrix Q -----------------------------------------
846 ✗ ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
847 ✗ if (ImGui::TreeNode(fmt::format("Q - System/Process noise covariance matrix##{}", size_t(id)).c_str()))
848 {
849 ✗ ImGui::SetNextItemWidth(configWidth + ImGui::GetStyle().ItemSpacing.x);
850
851 ✗ if (_imuFusionType == ImuFusionType::IRWKF)
852 {
853 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Angular acceleration ({})##{}",
854 ✗ _pinDataIRWKF.varAngularAccNoiseUnit == PinDataIRWKF::AngularAccVarianceUnit::rad2_s4
855 ✗ || _pinDataIRWKF.varAngularAccNoiseUnit == PinDataIRWKF::AngularAccVarianceUnit::deg2_s4
856 ✗ ? "Variance σ²"
857 : "Standard deviation σ",
858 ✗ size_t(id))
859 .c_str(),
860 ✗ configWidth, unitWidth, _pinDataIRWKF.varAngularAccNoise.data(), _pinDataIRWKF.varAngularAccNoiseUnit, "(rad^2)/(s^4)\0"
861 "rad/s^2\0"
862 "(deg^2)/(s^4)\0"
863 "deg/s^2\0\0",
864 "%.2e", ImGuiInputTextFlags_CharsScientific))
865 {
866 LOG_DATA("{}: varAngularAccNoise changed to {}", nameId(), _pinDataIRWKF.varAngularAccNoise.transpose());
867 LOG_DATA("{}: varAngularAccNoiseUnit changed to {}", nameId(), fmt::underlying(_pinDataIRWKF.varAngularAccNoiseUnit));
868 ✗ flow::ApplyChanges();
869 }
870
871 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Jerk ({})##{}",
872 ✗ _pinDataIRWKF.varJerkNoiseUnit == PinDataIRWKF::JerkVarianceUnit::m2_s6
873 ✗ ? "Variance σ²"
874 : "Standard deviation σ",
875 ✗ size_t(id))
876 .c_str(),
877 ✗ configWidth, unitWidth, _pinDataIRWKF.varJerkNoise.data(), _pinDataIRWKF.varJerkNoiseUnit, "(m^2)/(s^6)\0"
878 "m/s^3\0\0",
879 "%.2e", ImGuiInputTextFlags_CharsScientific))
880 {
881 LOG_DATA("{}: varJerkNoise changed to {}", nameId(), _pinDataIRWKF.varJerkNoise.transpose());
882 LOG_DATA("{}: varJerkNoiseUnit changed to {}", nameId(), fmt::underlying(_pinDataIRWKF.varJerkNoiseUnit));
883 ✗ flow::ApplyChanges();
884 }
885 }
886 else // (_imuFusionType == ImuFusionType::Bspline)
887 {
888 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Angular rate B-spline coefficients ({})##{}",
889 ✗ _pinDataBsplineKF.varCoeffsAngRateUnit == PinData::AngRateVarianceUnit::rad2_s2
890 ✗ || _pinDataBsplineKF.varCoeffsAngRateUnit == PinData::AngRateVarianceUnit::deg2_s2
891 ✗ ? "Variance σ²"
892 : "Standard deviation σ",
893 ✗ size_t(id))
894 .c_str(),
895 ✗ configWidth, unitWidth, _procNoiseCoeffsAngRateTemp.data(), _pinDataBsplineKF.varCoeffsAngRateUnit, "(rad^2)/(s^2)\0"
896 "rad/s\0"
897 "(deg^2)/(s^2)\0"
898 "deg/s\0\0",
899 "%.2e", ImGuiInputTextFlags_CharsScientific))
900 {
901 LOG_DATA("{}: varCoeffsAngRateUnit changed to {}", nameId(), fmt::underlying(_pinDataBsplineKF.varCoeffsAngRateUnit));
902 ✗ flow::ApplyChanges();
903 }
904 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Acceleration B-spline coefficients ({})##{}",
905 ✗ _pinDataBsplineKF.varCoeffsAccelUnit == PinData::AccelerationVarianceUnit::m2_s4
906 ✗ ? "Variance σ²"
907 : "Standard deviation σ",
908 ✗ size_t(id))
909 .c_str(),
910 ✗ configWidth, unitWidth, _procNoiseCoeffsAccelTemp.data(), _pinDataBsplineKF.varCoeffsAccelUnit, "(m^2)/(s^4)\0"
911 "m/s^2\0\0",
912 "%.2e", ImGuiInputTextFlags_CharsScientific))
913 {
914 LOG_DATA("{}: varCoeffsAccelUnit changed to {}", nameId(), fmt::underlying(_pinDataBsplineKF.varCoeffsAccelUnit));
915 ✗ flow::ApplyChanges();
916 }
917 ✗ for (uint8_t i = 0; i < _numBsplines; i += 3)
918 {
919 ✗ _pinDataBsplineKF.varCoeffsAngRateNoise.block<3, 1>(i, 0) = _procNoiseCoeffsAngRateTemp;
920 ✗ _pinDataBsplineKF.varCoeffsAccelNoise.block<3, 1>(i, 0) = _procNoiseCoeffsAccelTemp;
921 }
922 LOG_DATA("{}: varCoeffsAngRateNoise changed to {}", nameId(), _pinDataBsplineKF.varCoeffsAngRateNoise.transpose());
923 LOG_DATA("{}: varCoeffsAccelNoise changed to {}", nameId(), _pinDataBsplineKF.varCoeffsAccelNoise.transpose());
924 }
925
926 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Bias of the angular rate of sensor {} ({})##{}", 2,
927 ✗ _pinData[1].varBiasAngRateNoiseUnit == PinData::AngRateVarianceUnit::rad2_s2
928 ✗ || _pinData[1].varBiasAngRateNoiseUnit == PinData::AngRateVarianceUnit::deg2_s2
929 ✗ ? "Variance σ²"
930 : "Standard deviation σ",
931 ✗ size_t(id))
932 .c_str(), // FIXME: adapt config window number of sensors (if pin 3 is deleted, keep 1,2,4 instead of re-counting to 1,2,3)
933 ✗ configWidth, unitWidth, _pinData[1].varBiasAngRateNoise.data(), _pinData[1].varBiasAngRateNoiseUnit, "(rad/s)^2\0"
934 "rad/s\0"
935 "(deg/s)^2\0"
936 "deg/s\0\0",
937 "%.2e", ImGuiInputTextFlags_CharsScientific))
938 {
939 LOG_DATA("{}: varBiasAngRateNoise changed to {}", nameId(), _pinData[1].varBiasAngRateNoise.transpose());
940 LOG_DATA("{}: varBiasAngRateNoiseUnit changed to {}", nameId(), fmt::underlying(_pinData[1].varBiasAngRateNoiseUnit));
941 ✗ flow::ApplyChanges();
942 }
943
944 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Bias of the acceleration of sensor {} ({})##{}", 2,
945 ✗ _pinData[1].varBiasAccelerationNoiseUnit == PinData::AccelerationVarianceUnit::m2_s4
946 ✗ ? "Variance σ²"
947 : "Standard deviation σ",
948 ✗ size_t(id))
949 .c_str(), // FIXME: adapt config window number of sensors (if pin 3 is deleted, keep 1,2,4 instead of re-counting to 1,2,3)
950 ✗ configWidth, unitWidth, _pinData[1].varBiasAccelerationNoise.data(), _pinData[1].varBiasAccelerationNoiseUnit, "(m^2)/(s^4)\0"
951 "m/s^2\0\0",
952 "%.2e", ImGuiInputTextFlags_CharsScientific))
953 {
954 LOG_DATA("{}: varBiasAccelerationNoise changed to {}", nameId(), _pinData[1].varBiasAccelerationNoise.transpose());
955 LOG_DATA("{}: varBiasAccelerationNoiseUnit changed to {}", nameId(), fmt::underlying(_pinData[1].varBiasAccelerationNoiseUnit));
956 ✗ flow::ApplyChanges();
957 }
958 ✗ if (!_imuCharacteristicsIdentical)
959 {
960 ✗ for (size_t pinIndex = 2; pinIndex < _nInputPins; ++pinIndex)
961 {
962 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Bias of the angular rate of sensor {} ({})##{}", pinIndex + 1,
963 ✗ _pinData[pinIndex].varBiasAngRateNoiseUnit == PinData::AngRateVarianceUnit::rad2_s2
964 ✗ || _pinData[pinIndex].varBiasAngRateNoiseUnit == PinData::AngRateVarianceUnit::deg2_s2
965 ✗ ? "Variance σ²"
966 : "Standard deviation σ",
967 ✗ size_t(id))
968 .c_str(), // FIXME: adapt config window number of sensors (if pin 3 is deleted, keep 1,2,4 instead of re-counting to 1,2,3)
969 ✗ configWidth, unitWidth, _pinData[pinIndex].varBiasAngRateNoise.data(), _pinData[pinIndex].varBiasAngRateNoiseUnit, "(rad/s)^2\0"
970 "rad/s\0"
971 "(deg/s)^2\0"
972 "deg/s\0\0",
973 "%.2e", ImGuiInputTextFlags_CharsScientific))
974 {
975 LOG_DATA("{}: varBiasAngRateNoise changed to {}", nameId(), _pinData[pinIndex].varBiasAngRateNoise.transpose());
976 LOG_DATA("{}: varBiasAngRateNoiseUnit changed to {}", nameId(), fmt::underlying(_pinData[pinIndex].varBiasAngRateNoiseUnit));
977 ✗ flow::ApplyChanges();
978 }
979
980 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Bias of the acceleration of sensor {} ({})##{}", pinIndex + 1,
981 ✗ _pinData[pinIndex].varBiasAccelerationNoiseUnit == PinData::AccelerationVarianceUnit::m2_s4
982 ✗ ? "Variance σ²"
983 : "Standard deviation σ",
984 ✗ size_t(id))
985 .c_str(), // FIXME: adapt config window number of sensors (if pin 3 is deleted, keep 1,2,4 instead of re-counting to 1,2,3)
986 ✗ configWidth, unitWidth, _pinData[pinIndex].varBiasAccelerationNoise.data(), _pinData[pinIndex].varBiasAccelerationNoiseUnit, "(m^2)/(s^4)\0"
987 "m/s^2\0\0",
988 "%.2e", ImGuiInputTextFlags_CharsScientific))
989 {
990 LOG_DATA("{}: varBiasAccelerationNoise changed to {}", nameId(), _pinData[pinIndex].varBiasAccelerationNoise.transpose());
991 LOG_DATA("{}: varBiasAccelerationNoiseUnit changed to {}", nameId(), fmt::underlying(_pinData[pinIndex].varBiasAccelerationNoiseUnit));
992 ✗ flow::ApplyChanges();
993 }
994 }
995 }
996
997 ✗ ImGui::TreePop();
998 }
999
1000 // ------------------------------------ Measurement noise matrix R ---------------------------------------
1001 ✗ ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
1002 ✗ if (ImGui::TreeNode(fmt::format("R - Measurement noise covariance matrix##{}", size_t(id)).c_str()))
1003 {
1004 ✗ ImGui::SetNextItemWidth(configWidth + ImGui::GetStyle().ItemSpacing.x);
1005
1006 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Angular rate of sensor {} ({})##{}", 1,
1007 ✗ _pinData[0].measurementUncertaintyAngularRateUnit == PinData::AngRateVarianceUnit::rad2_s2
1008 ✗ || _pinData[0].measurementUncertaintyAngularRateUnit == PinData::AngRateVarianceUnit::deg2_s2
1009 ✗ ? "Variance σ²"
1010 : "Standard deviation σ",
1011 ✗ size_t(id))
1012 .c_str(),
1013 ✗ configWidth, unitWidth, _pinData[0].measurementUncertaintyAngularRate.data(), _pinData[0].measurementUncertaintyAngularRateUnit, "(rad/s)^2\0"
1014 "rad/s\0"
1015 "(deg/s)^2\0"
1016 "deg/s\0\0",
1017 "%.2e", ImGuiInputTextFlags_CharsScientific))
1018 {
1019 LOG_DATA("{}: stdevAngularAcc changed to {}", nameId(), _pinData[0].measurementUncertaintyAngularRate.transpose());
1020 LOG_DATA("{}: stdevAngularAccUnit changed to {}", nameId(), fmt::underlying(_pinData[0].measurementUncertaintyAngularRateUnit));
1021 ✗ flow::ApplyChanges();
1022 }
1023
1024 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Acceleration of sensor {} ({})##{}", 1,
1025 ✗ _pinData[0].measurementUncertaintyAccelerationUnit == PinData::AccelerationVarianceUnit::m2_s4
1026 ✗ ? "Variance σ²"
1027 : "Standard deviation σ",
1028 ✗ size_t(id))
1029 .c_str(),
1030 ✗ configWidth, unitWidth, _pinData[0].measurementUncertaintyAcceleration.data(), _pinData[0].measurementUncertaintyAccelerationUnit, "(m^2)/(s^4)\0"
1031 "m/s^2\0\0",
1032 "%.2e", ImGuiInputTextFlags_CharsScientific))
1033 {
1034 LOG_DATA("{}: stdevJerk changed to {}", nameId(), _pinData[0].measurementUncertaintyAcceleration.transpose());
1035 LOG_DATA("{}: stdevJerkUnit changed to {}", nameId(), fmt::underlying(_pinData[0].measurementUncertaintyAccelerationUnit));
1036 ✗ flow::ApplyChanges();
1037 }
1038 ✗ if (!_imuCharacteristicsIdentical)
1039 {
1040 ✗ for (size_t pinIndex = 1; pinIndex < _nInputPins; ++pinIndex)
1041 {
1042 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Angular rate of sensor {} ({})##{}", pinIndex + 1,
1043 ✗ _pinData[pinIndex].measurementUncertaintyAngularRateUnit == PinData::AngRateVarianceUnit::rad2_s2
1044 ✗ || _pinData[pinIndex].measurementUncertaintyAngularRateUnit == PinData::AngRateVarianceUnit::deg2_s2
1045 ✗ ? "Variance σ²"
1046 : "Standard deviation σ",
1047 ✗ size_t(id))
1048 .c_str(),
1049 ✗ configWidth, unitWidth, _pinData[pinIndex].measurementUncertaintyAngularRate.data(), _pinData[pinIndex].measurementUncertaintyAngularRateUnit, "(rad/s)^2\0"
1050 "rad/s\0"
1051 "(deg/s)^2\0"
1052 "deg/s\0\0",
1053 "%.2e", ImGuiInputTextFlags_CharsScientific))
1054 {
1055 LOG_DATA("{}: stdevAngularAcc changed to {}", nameId(), _pinData[pinIndex].measurementUncertaintyAngularRate.transpose());
1056 LOG_DATA("{}: stdevAngularAccUnit changed to {}", nameId(), fmt::underlying(_pinData[pinIndex].measurementUncertaintyAngularRateUnit));
1057 ✗ flow::ApplyChanges();
1058 }
1059
1060 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Acceleration of sensor {} ({})##{}", pinIndex + 1,
1061 ✗ _pinData[pinIndex].measurementUncertaintyAccelerationUnit == PinData::AccelerationVarianceUnit::m2_s4
1062 ✗ ? "Variance σ²"
1063 : "Standard deviation σ",
1064 ✗ size_t(id))
1065 .c_str(),
1066 ✗ configWidth, unitWidth, _pinData[pinIndex].measurementUncertaintyAcceleration.data(), _pinData[pinIndex].measurementUncertaintyAccelerationUnit, "(m^2)/(s^4)\0"
1067 "m/s^2\0\0",
1068 "%.2e", ImGuiInputTextFlags_CharsScientific))
1069 {
1070 LOG_DATA("{}: stdevJerk changed to {}", nameId(), _pinData[pinIndex].measurementUncertaintyAcceleration.transpose());
1071 LOG_DATA("{}: stdevJerkUnit changed to {}", nameId(), fmt::underlying(_pinData[pinIndex].measurementUncertaintyAccelerationUnit));
1072 ✗ flow::ApplyChanges();
1073 }
1074 }
1075 }
1076
1077 ✗ ImGui::TreePop();
1078 }
1079 ✗ }
1080
1081 ✗ [[nodiscard]] json NAV::ImuFusion::save() const
1082 {
1083 LOG_TRACE("{}: called", nameId());
1084
1085 ✗ json j;
1086
1087 ✗ j["imuFusionType"] = _imuFusionType;
1088 ✗ j["checkKalmanMatricesRanks"] = _checkKalmanMatricesRanks;
1089 ✗ j["nInputPins"] = _nInputPins;
1090 ✗ j["imuFrequency"] = _imuFrequency;
1091 ✗ j["numStates"] = _numStates;
1092 ✗ j["pinData"] = _pinData;
1093 ✗ j["pinDataIRWKF"] = _pinDataIRWKF;
1094 ✗ j["pinDataBsplineKF"] = _pinDataBsplineKF;
1095 ✗ j["initCoeffsAngRateTemp"] = _initCoeffsAngRateTemp;
1096 ✗ j["initCoeffsAccelTemp"] = _initCoeffsAccelTemp;
1097 ✗ j["initCovarianceCoeffsAngRateTemp"] = _initCovarianceCoeffsAngRateTemp;
1098 ✗ j["initCovarianceCoeffsAccelTemp"] = _initCovarianceCoeffsAccelTemp;
1099 ✗ j["procNoiseCoeffsAngRateTemp"] = _procNoiseCoeffsAngRateTemp;
1100 ✗ j["procNoiseCoeffsAccelTemp"] = _procNoiseCoeffsAccelTemp;
1101 ✗ j["autoInitKF"] = _autoInitKF;
1102 ✗ j["initJerkAngAcc"] = _initJerkAngAcc;
1103 ✗ j["kfInitialized"] = _kfInitialized;
1104 ✗ j["averageEndTime"] = _averageEndTime;
1105 ✗ j["splineSpacing"] = _splineSpacing;
1106
1107 ✗ return j;
1108 ✗ }
1109
1110 2 void NAV::ImuFusion::restore(json const& j)
1111 {
1112 LOG_TRACE("{}: called", nameId());
1113
1114
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("imuFusionType"))
1115 {
1116 2 j.at("imuFusionType").get_to(_imuFusionType);
1117 }
1118
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("checkKalmanMatricesRanks"))
1119 {
1120 2 j.at("checkKalmanMatricesRanks").get_to(_checkKalmanMatricesRanks);
1121 }
1122
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("nInputPins"))
1123 {
1124 2 j.at("nInputPins").get_to(_nInputPins);
1125 2 updateNumberOfInputPins();
1126 }
1127
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("imuFrequency"))
1128 {
1129 2 j.at("imuFrequency").get_to(_imuFrequency);
1130 }
1131
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("numStates"))
1132 {
1133 2 j.at("numStates").get_to(_numStates);
1134 }
1135
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("pinData"))
1136 {
1137 2 j.at("pinData").get_to(_pinData);
1138 }
1139
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("pinDataIRWKF"))
1140 {
1141 2 j.at("pinDataIRWKF").get_to(_pinDataIRWKF);
1142 }
1143
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("pinDataBsplineKF"))
1144 {
1145 2 j.at("pinDataBsplineKF").get_to(_pinDataBsplineKF);
1146 }
1147
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("initCoeffsAngRateTemp"))
1148 {
1149 2 j.at("initCoeffsAngRateTemp").get_to(_initCoeffsAngRateTemp);
1150 }
1151
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("initCoeffsAccelTemp"))
1152 {
1153 2 j.at("initCoeffsAccelTemp").get_to(_initCoeffsAccelTemp);
1154 }
1155
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("initCovarianceCoeffsAngRateTemp"))
1156 {
1157 2 j.at("initCovarianceCoeffsAngRateTemp").get_to(_initCovarianceCoeffsAngRateTemp);
1158 }
1159
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("initCovarianceCoeffsAccelTemp"))
1160 {
1161 2 j.at("initCovarianceCoeffsAccelTemp").get_to(_initCovarianceCoeffsAccelTemp);
1162 }
1163
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("procNoiseCoeffsAngRateTemp"))
1164 {
1165 2 j.at("procNoiseCoeffsAngRateTemp").get_to(_procNoiseCoeffsAngRateTemp);
1166 }
1167
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("procNoiseCoeffsAccelTemp"))
1168 {
1169 2 j.at("procNoiseCoeffsAccelTemp").get_to(_procNoiseCoeffsAccelTemp);
1170 }
1171
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("autoInitKF"))
1172 {
1173 2 j.at("autoInitKF").get_to(_autoInitKF);
1174 }
1175
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("initJerkAngAcc"))
1176 {
1177 2 j.at("initJerkAngAcc").get_to(_initJerkAngAcc);
1178 }
1179
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 if (j.contains("_kfInitialized"))
1180 {
1181 ✗ j.at("_kfInitialized").get_to(_kfInitialized);
1182 }
1183
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("averageEndTime"))
1184 {
1185 2 j.at("averageEndTime").get_to(_averageEndTime);
1186 }
1187
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("splineSpacing"))
1188 {
1189 2 j.at("splineSpacing").get_to(_splineSpacing);
1190 }
1191 2 }
1192
1193 6 bool NAV::ImuFusion::initialize()
1194 {
1195 LOG_TRACE("{}: called", nameId());
1196
1197 6 _imuRotations_accel.clear();
1198 6 _imuRotations_gyro.clear();
1199 6 _biasCovariances.clear();
1200 6 _processNoiseVariances.clear();
1201 6 _measurementNoiseVariances.clear();
1202
1203 6 _cumulatedImuObs.clear();
1204 6 _cumulatedPinIds.clear();
1205 6 _lastFiltObs.reset();
1206 6 _latestTimestamp = InsTime{};
1207 6 _firstTimestamp.reset();
1208
1209 6 _imuPosSet = false;
1210 6 _kfInitialized = false;
1211
1212
2/2
✓ Branch 1 taken 27 times.
✓ Branch 2 taken 6 times.
 33 for (size_t pinIndex = 0; pinIndex < _pinData.size(); pinIndex++)
1213 {
1214
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 27 times.
 27 if (!inputPins.at(pinIndex).isPinLinked())
1215 {
1216 ✗ LOG_INFO("Fewer links than input pins - Consider deleting pins that are not connected to limit KF matrices to the necessary size.");
1217 }
1218 }
1219
1220
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 3 times.
 6 _numStatesEst = _imuFusionType == ImuFusionType::IRWKF ? _numStatesEstIRWKF : _numStatesEstBsplineKF;
1221
1222 6 _numStates = _numStatesEst + static_cast<uint8_t>((_nInputPins - 1) * _numStatesPerPin);
1223
1224
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 _kalmanFilter = KalmanFilter{ _numStates, _numMeasurements };
1225 6 _kalmanFilter.setZero();
1226
1227 6 initializeMountingAngles();
1228
1229 // --------------------------------------------------------- KF Initializations ------------------------------------------------------------
1230
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 3 times.
 6 if (!_autoInitKF) // i.e. manual initialization thru inputs from the GUI
1231 {
1232 3 auto dtInit = 1.0 / _imuFrequency; // Initial state transition time in [s]
1233
1234
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
 3 if (_imuFusionType == ImuFusionType::IRWKF)
1235 {
1236 ✗ _kalmanFilter = IRWKF::initializeKalmanFilterManually(_nInputPins, _pinData, _pinDataIRWKF, _numStates, dtInit, _processNoiseVariances, _kalmanFilter, _imuCharacteristicsIdentical, _imuBiasesIdentical);
1237 }
1238 else // (_imuFusionType == ImuFusionType::BsplineKF)
1239 {
1240
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 _kalmanFilter = BsplineKF::initializeKalmanFilterManually(_nInputPins, _pinData, _pinDataBsplineKF, _numStates, dtInit, _processNoiseVariances, _kalmanFilter, _imuCharacteristicsIdentical, _imuBiasesIdentical);
1241 3 _latestKnot = 0.0;
1242 }
1243
1244 LOG_DATA("{}: Initial kalmanFilter.x = {}", nameId(), _kalmanFilter.x.transpose());
1245 LOG_DATA("{}: Initial kalmanFilter.P =\n{}", nameId(), _kalmanFilter.P);
1246 LOG_DATA("{}: Initial kalmanFilter.Phi =\n{}", nameId(), _kalmanFilter.Phi);
1247 LOG_DATA("{}: Initial kalmanFilter.Q =\n{}", nameId(), _kalmanFilter.Q);
1248 }
1249
1250 // -------------------------------------------------- Measurement uncertainty matrix R -----------------------------------------------------
1251 6 _measurementNoiseVariances.resize(2 * _nInputPins);
1252
1253 6 size_t pinDataIdx = 0;
1254
2/2
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 6 times.
 33 for (size_t pinIndex = 0; pinIndex < _nInputPins; ++pinIndex)
1255 {
1256
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 27 times.
 27 if (!_imuCharacteristicsIdentical)
1257 {
1258 ✗ pinDataIdx = pinIndex;
1259 }
1260
1261 // Measurement uncertainty for the angular rate (Variance σ²) in [(rad/s)^2, (rad/s)^2, (rad/s)^2]
1262
1/5
✗ Branch 1 not taken.
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 27 switch (_pinData[pinDataIdx].measurementUncertaintyAngularRateUnit)
1263 {
1264 ✗ case PinData::AngRateVarianceUnit::rad_s:
1265 ✗ _measurementNoiseVariances[2 * pinIndex] = (_pinData[pinDataIdx].measurementUncertaintyAngularRate).array().pow(2);
1266 ✗ break;
1267 27 case PinData::AngRateVarianceUnit::deg_s:
1268
4/8
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 27 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 27 times.
✗ Branch 13 not taken.
 27 _measurementNoiseVariances[2 * pinIndex] = (deg2rad(_pinData[pinDataIdx].measurementUncertaintyAngularRate)).array().pow(2);
1269 27 break;
1270 ✗ case PinData::AngRateVarianceUnit::rad2_s2:
1271 ✗ _measurementNoiseVariances[2 * pinIndex] = _pinData[pinDataIdx].measurementUncertaintyAngularRate;
1272 ✗ break;
1273 ✗ case PinData::AngRateVarianceUnit::deg2_s2:
1274 ✗ _measurementNoiseVariances[2 * pinIndex] = deg2rad((_pinData[pinDataIdx].measurementUncertaintyAngularRate).cwiseSqrt()).array().pow(2);
1275 ✗ break;
1276 }
1277
1278 // Measurement uncertainty for the acceleration (Variance σ²) in [(m^2)/(s^4), (m^2)/(s^4), (m^2)/(s^4)]
1279
1/3
✗ Branch 1 not taken.
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
 27 switch (_pinData[pinDataIdx].measurementUncertaintyAccelerationUnit)
1280 {
1281 ✗ case PinData::AccelerationVarianceUnit::m2_s4:
1282 ✗ _measurementNoiseVariances[1 + 2 * pinIndex] = _pinData[pinDataIdx].measurementUncertaintyAcceleration;
1283 ✗ break;
1284 27 case PinData::AccelerationVarianceUnit::m_s2:
1285
3/6
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 27 times.
✗ Branch 10 not taken.
 27 _measurementNoiseVariances[1 + 2 * pinIndex] = (_pinData[pinDataIdx].measurementUncertaintyAcceleration).array().pow(2);
1286 27 break;
1287 }
1288 }
1289
1290
1/2
✓ Branch 0 taken 6 times.
✗ Branch 1 not taken.
 6 if (_imuCharacteristicsIdentical)
1291 {
1292 6 measurementNoiseMatrix_R(_kalmanFilter.R);
1293 LOG_DATA("{}: imuCharacteristicsIdentical - kalmanFilter.R =\n{}", nameId(), _kalmanFilter.R);
1294 }
1295
1296
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 3 times.
 6 if (!_autoInitKF) { _kfInitialized = true; } // Auto-init initializes KF at 'avgEndTime' seconds --> see 'recvSignal'
1297
1298
1/2
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 12 LOG_DEBUG("ImuFusion initialized");
1299
1300 6 return true;
1301 }
1302
1303 2 void NAV::ImuFusion::deinitialize()
1304 {
1305 LOG_TRACE("{}: called", nameId());
1306 2 }
1307
1308 118 void NAV::ImuFusion::updateNumberOfInputPins()
1309 {
1310
2/2
✓ Branch 1 taken 237 times.
✓ Branch 2 taken 118 times.
 473 while (inputPins.size() < _nInputPins)
1311 {
1312
4/8
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
✓ Branch 7 taken 237 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 237 times.
✓ Branch 12 taken 237 times.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
 948 CreateInputPin(fmt::format("Pin {}", inputPins.size() + 1).c_str(), Pin::Type::Flow,
1313 { NAV::ImuObs::type() }, &ImuFusion::recvSignal);
1314 237 _pinData.emplace_back();
1315
2/2
✓ Branch 1 taken 121 times.
✓ Branch 2 taken 116 times.
 237 if (outputPins.size() < _nInputPins)
1316 {
1317
4/8
✓ Branch 2 taken 121 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 121 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 121 times.
✓ Branch 13 taken 121 times.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
 605 CreateOutputPin(fmt::format("ImuBiases {}1", outputPins.size() + 1).c_str(), Pin::Type::Flow, { NAV::InsGnssLCKFSolution::type() });
1318 }
1319 }
1320
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 118 times.
 118 while (inputPins.size() > _nInputPins) // TODO: while loop still necessary here? guiConfig also deletes pins
1321 {
1322 ✗ DeleteInputPin(inputPins.size() - 1);
1323 ✗ DeleteOutputPin(outputPins.size() - 1);
1324 ✗ _pinData.pop_back();
1325 }
1326 118 _pinData.resize(_nInputPins);
1327 118 initializeMountingAngles();
1328 476 }
1329
1330 124 void NAV::ImuFusion::initializeMountingAngles()
1331 {
1332 124 _imuRotations_accel.resize(_nInputPins);
1333 124 _imuRotations_gyro.resize(_nInputPins);
1334
2/2
✓ Branch 0 taken 268 times.
✓ Branch 1 taken 124 times.
 392 for (size_t i = 0; i < _nInputPins; ++i)
1335 {
1336
2/4
✓ Branch 1 taken 268 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 268 times.
✗ Branch 6 not taken.
 268 _imuRotations_accel[i] = Eigen::Matrix3d::Zero();
1337
2/4
✓ Branch 1 taken 268 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 268 times.
✗ Branch 6 not taken.
 268 _imuRotations_gyro[i] = Eigen::Matrix3d::Zero();
1338
1339 // Assigning nan for an efficient check during runtime, whether mounting angles have been read for sensor i
1340 268 _imuRotations_accel[i](0, 0) = std::nan("");
1341 268 _imuRotations_gyro[i](0, 0) = std::nan("");
1342 }
1343 124 }
1344
1345 17486 void NAV::ImuFusion::recvSignal(NAV::InputPin::NodeDataQueue& queue, size_t pinIdx)
1346 {
1347
1/2
✓ Branch 1 taken 17486 times.
✗ Branch 2 not taken.
 17486 auto imuObs = std::static_pointer_cast<const ImuObs>(queue.extract_front());
1348
1349
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 17486 times.
 17486 if (imuObs->insTime.empty())
1350 {
1351 ✗ LOG_ERROR("{}: Can't set new imuObs__t0 because the observation has no time tag (insTime/timeSinceStartup)", nameId());
1352 ✗ return;
1353 }
1354
1355
2/2
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 17484 times.
 17486 if (_latestTimestamp.empty())
1356 {
1357 // Initial time step for KF prediction
1358
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 InsTime dt_init = InsTime{ 0, 0, 0, 0, 0, 1.0 / _imuFrequency };
1359
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 _latestTimestamp = InsTime{ 0, 0, 0, 0, 0, (imuObs->insTime - dt_init).count() };
1360 2 _firstTimestamp = imuObs->insTime;
1361
1362 // Time until averaging ends and filtering starts (for auto-init of KF)
1363
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
 2 _avgEndTime = imuObs->insTime + std::chrono::milliseconds(static_cast<int>(1e3 * _averageEndTime));
1364 }
1365
1366
1/2
✓ Branch 2 taken 17486 times.
✗ Branch 3 not taken.
 17486 _timeSinceStartup = static_cast<double>((imuObs->insTime - _firstTimestamp).count());
1367 LOG_DATA("_timeSinceStartup = {}", _timeSinceStartup);
1368
1369 // Predict states over the time difference between the latest signal and the one before
1370
1/2
✓ Branch 2 taken 17486 times.
✗ Branch 3 not taken.
 17486 auto dt = static_cast<double>((imuObs->insTime - _latestTimestamp).count());
1371 17486 _latestTimestamp = imuObs->insTime;
1372 LOG_DATA("{}: dt = {}", nameId(), dt);
1373
1374
2/2
✓ Branch 0 taken 17385 times.
✓ Branch 1 taken 101 times.
 17486 if (_kfInitialized)
1375 {
1376
2/2
✓ Branch 0 taken 9903 times.
✓ Branch 1 taken 7482 times.
 17385 if (_imuFusionType == ImuFusionType::IRWKF)
1377 {
1378
1/2
✓ Branch 1 taken 9903 times.
✗ Branch 2 not taken.
 9903 IRWKF::stateTransitionMatrix_Phi(_kalmanFilter.Phi, dt);
1379
1/2
✓ Branch 1 taken 9903 times.
✗ Branch 2 not taken.
 9903 IRWKF::processNoiseMatrix_Q(_kalmanFilter.Q, dt, _processNoiseVariances, _numStates);
1380 }
1381 else // (_imuFusionType == ImuFusionType::Bspline)
1382 {
1383
1/2
✓ Branch 1 taken 7482 times.
✗ Branch 2 not taken.
 7482 BsplineKF::processNoiseMatrix_Q(_kalmanFilter.Q, dt, _processNoiseVariances, _numStates);
1384
1385
2/2
✓ Branch 0 taken 61 times.
✓ Branch 1 taken 7421 times.
 7482 if (_timeSinceStartup >= _latestKnot)
1386 {
1387 61 _latestKnot += _splineSpacing;
1388
1389
1/2
✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
 61 BsplineKF::rotateCoeffStates(_kalmanFilter.x);
1390 LOG_DATA("{}: kalmanFilter.P before B-spline coeff rotation =\n{}", nameId(), _kalmanFilter.P.block<18, 18>(0, 0));
1391
1/2
✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
 61 BsplineKF::rotateErrorCovariances(_kalmanFilter.P, _numStates);
1392 }
1393 }
1394
1395 LOG_DATA("{}: kalmanFilter.P (B-spline coeffs) =\n{}", nameId(), _kalmanFilter.P.block<18, 18>(0, 0));
1396 LOG_DATA("{}: kalmanFilter.Phi =\n{}", nameId(), _kalmanFilter.Phi);
1397 LOG_DATA("{}: kalmanFilter.Q =\n{}", nameId(), _kalmanFilter.Q);
1398
1399
2/2
✓ Branch 0 taken 7482 times.
✓ Branch 1 taken 9903 times.
 17385 if (_checkKalmanMatricesRanks)
1400 {
1401
3/6
✓ Branch 2 taken 7482 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7482 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 7482 times.
✗ Branch 8 not taken.
 7482 if (inputPins.at(_pinData.size() - 1).isPinLinked())
1402 {
1403
2/4
✓ Branch 1 taken 7482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7482 times.
✗ Branch 5 not taken.
 7482 auto rank = _kalmanFilter.P.fullPivLu().rank();
1404
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 7482 times.
 7482 if (rank != _kalmanFilter.P.rows())
1405 {
1406 ✗ LOG_WARN("{}: P.rank = {}", nameId(), rank);
1407 }
1408 }
1409 }
1410 }
1411
1412 // Read sensor rotation info from 'imuObs'
1413
3/4
✓ Branch 2 taken 17486 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✓ Branch 6 taken 17477 times.
 17486 if (std::isnan(_imuRotations_accel[pinIdx](0, 0)))
1414 {
1415 // Rotation matrix of the accelerometer platform to body frame
1416
1/2
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
 9 _imuRotations_accel[pinIdx] = imuObs->imuPos.b_quat_p().toRotationMatrix();
1417 }
1418
3/4
✓ Branch 2 taken 17486 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✓ Branch 6 taken 17477 times.
 17486 if (std::isnan(_imuRotations_gyro[pinIdx](0, 0)))
1419 {
1420 // Rotation matrix of the gyro platform to body frame
1421
1/2
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
 9 _imuRotations_gyro[pinIdx] = imuObs->imuPos.b_quat_p().toRotationMatrix();
1422 }
1423
1424 // Initialize H with mounting angles (DCM) of the sensor that provided the latest measurement
1425
2/4
✓ Branch 1 taken 17486 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17486 times.
✗ Branch 5 not taken.
 17486 auto DCM_accel = _imuRotations_accel.at(pinIdx);
1426 LOG_DATA("DCM_accel =\n{}", DCM_accel);
1427
2/4
✓ Branch 1 taken 17486 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17486 times.
✗ Branch 5 not taken.
 17486 auto DCM_gyro = _imuRotations_gyro.at(pinIdx);
1428 LOG_DATA("{}: DCM_gyro =\n{}", nameId(), DCM_gyro);
1429
1430
2/2
✓ Branch 0 taken 10004 times.
✓ Branch 1 taken 7482 times.
 17486 if (_imuFusionType == ImuFusionType::IRWKF)
1431 {
1432
1/2
✓ Branch 1 taken 10004 times.
✗ Branch 2 not taken.
 10004 _kalmanFilter.H = IRWKF::designMatrix_H(DCM_accel, DCM_gyro, pinIdx, _numMeasurements, _numStates, _numStatesEst, _numStatesPerPin);
1433 LOG_DATA("{}: Sensor (pinIdx): {}, kalmanFilter.H =\n{}", nameId(), _kalmanFilter.H, pinIdx);
1434 }
1435 else // (_imuFusionType == ImuFusionType::BsplineKF)
1436 {
1437
1/2
✓ Branch 1 taken 7482 times.
✗ Branch 2 not taken.
 7482 _kalmanFilter.H = BsplineKF::designMatrix_H(_timeSinceStartup, _splineSpacing, DCM_accel, DCM_gyro, pinIdx, _numMeasurements, _numStates, _numStatesEst, _numStatesPerPin);
1438 LOG_DATA("{}: timeSinceStartup: {}, Sensor (pinIdx): {}, kalmanFilter.H =\n{}", nameId(), _timeSinceStartup, pinIdx, _kalmanFilter.H);
1439 }
1440
1441
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 17486 times.
 17486 if (!_imuCharacteristicsIdentical)
1442 {
1443 ✗ measurementNoiseMatrix_R(_kalmanFilter.R, pinIdx);
1444 LOG_DATA("{}: kalmanFilter.R =\n{}", nameId(), _kalmanFilter.R);
1445 }
1446
1447
2/2
✓ Branch 0 taken 7482 times.
✓ Branch 1 taken 10004 times.
 17486 if (_checkKalmanMatricesRanks)
1448 {
1449
6/12
✓ Branch 1 taken 7482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7482 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7482 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7482 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7482 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 7482 times.
✗ Branch 17 not taken.
 7482 auto rank = (_kalmanFilter.H * _kalmanFilter.P * _kalmanFilter.H.transpose() + _kalmanFilter.R).fullPivLu().rank();
1450
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 7482 times.
 7482 if (rank != _kalmanFilter.H.rows())
1451 {
1452 ✗ LOG_WARN("{}: (HPH^T + R).rank = {}", nameId(), rank);
1453 }
1454 }
1455
1456
4/4
✓ Branch 0 taken 10004 times.
✓ Branch 1 taken 7482 times.
✓ Branch 2 taken 101 times.
✓ Branch 3 taken 9903 times.
 17486 if (_autoInitKF && !_kfInitialized)
1457 {
1458
2/2
✓ Branch 2 taken 100 times.
✓ Branch 3 taken 1 times.
 101 if (imuObs->insTime < _avgEndTime)
1459 {
1460
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 100 _cumulatedImuObs.push_back(imuObs);
1461
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 100 _cumulatedPinIds.push_back(pinIdx);
1462 }
1463 else // if (imuObs->insTime == _avgEndTime) // <-- do auto-init, once _avgEndTime is reached
1464 {
1465 1 double dtInit = 1.0 / _imuFrequency;
1466
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 _kalmanFilter = IRWKF::initializeKalmanFilterAuto(_nInputPins, _pinData, _pinDataIRWKF, _cumulatedPinIds, _cumulatedImuObs, _initJerkAngAcc, dtInit, _numStates, _numMeasurements, _processNoiseVariances, _kalmanFilter);
1467 1 _kfInitialized = true; // Start Kalman Filter
1468 }
1469 }
1470
2/2
✓ Branch 0 taken 17386 times.
✓ Branch 1 taken 100 times.
 17486 if (_kfInitialized)
1471 {
1472
1/2
✓ Branch 1 taken 17386 times.
✗ Branch 2 not taken.
 17386 combineSignals(imuObs);
1473 }
1474 17486 }
1475
1476 17386 void NAV::ImuFusion::combineSignals(const std::shared_ptr<const ImuObs>& imuObs)
1477 {
1478 LOG_DATA("{}: called", nameId());
1479
1480
1/2
✓ Branch 1 taken 17386 times.
✗ Branch 2 not taken.
 17386 auto imuObsFiltered = std::make_shared<ImuObs>(this->_imuPos);
1481
1482 LOG_DATA("{}: Estimated state before prediction: x =\n{}", nameId(), _kalmanFilter.x);
1483
1484
1/2
✓ Branch 1 taken 17386 times.
✗ Branch 2 not taken.
 17386 _kalmanFilter.predict();
1485
1486 LOG_DATA("{}: kalmanFilter.P (B-spline coeffs) =\n{}", nameId(), _kalmanFilter.P.block<18, 18>(0, 0));
1487
1488 LOG_DATA("{}: kalmanFilter.P (B-spline coeffs) =\n{}", nameId(), _kalmanFilter.P.block<18, 18>(0, 0));
1489
1490
2/4
✓ Branch 2 taken 17386 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 17386 times.
✗ Branch 6 not taken.
 17386 _kalmanFilter.z.block<3, 1>(0, 0) = imuObs->p_angularRate;
1491
2/4
✓ Branch 2 taken 17386 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 17386 times.
✗ Branch 6 not taken.
 17386 _kalmanFilter.z.block<3, 1>(3, 0) = imuObs->p_acceleration;
1492
1493 LOG_DATA("{}: Measurements z =\n{}", nameId(), _kalmanFilter.z);
1494 LOG_DATA("{}: coeff states x =\n{}", nameId(), _kalmanFilter.x.block<18, 1>(0, 0));
1495 LOG_DATA("{}: Innovation: z - H * x =\n{}", nameId(), _kalmanFilter.z - _kalmanFilter.H * _kalmanFilter.x);
1496
1497
1/2
✓ Branch 1 taken 17386 times.
✗ Branch 2 not taken.
 17386 _kalmanFilter.correct();
1498 LOG_DATA("{}: Estimated state after correction: x =\n{}", nameId(), _kalmanFilter.x);
1499
1500
2/2
✓ Branch 0 taken 7482 times.
✓ Branch 1 taken 9904 times.
 17386 if (_checkKalmanMatricesRanks)
1501 {
1502
6/12
✓ Branch 1 taken 7482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7482 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7482 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7482 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7482 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 7482 times.
✗ Branch 17 not taken.
 7482 auto rankH = (_kalmanFilter.H * _kalmanFilter.P * _kalmanFilter.H.transpose() + _kalmanFilter.R).fullPivLu().rank();
1503
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 7482 times.
 7482 if (rankH != _kalmanFilter.H.rows())
1504 {
1505 ✗ LOG_WARN("{}: (HPH^T + R).rank = {}", nameId(), rankH);
1506 }
1507
1508
3/6
✓ Branch 2 taken 7482 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7482 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 7482 times.
✗ Branch 8 not taken.
 7482 if (inputPins.at(_pinData.size() - 1).isPinLinked())
1509 {
1510
2/4
✓ Branch 1 taken 7482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7482 times.
✗ Branch 5 not taken.
 7482 auto rankP = _kalmanFilter.P.fullPivLu().rank();
1511
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 7482 times.
 7482 if (rankP != _kalmanFilter.P.rows())
1512 {
1513 ✗ LOG_WARN("{}: P.rank = {}", nameId(), rankP);
1514 LOG_DATA("{}: kalmanFilter.P =\n{}", nameId(), _kalmanFilter.P);
1515 }
1516 }
1517 }
1518
1519 // Construct imuObs
1520 17386 imuObsFiltered->insTime = imuObs->insTime;
1521
2/2
✓ Branch 0 taken 9904 times.
✓ Branch 1 taken 7482 times.
 17386 if (_imuFusionType == ImuFusionType::IRWKF)
1522 {
1523
4/8
✓ Branch 1 taken 9904 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9904 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9904 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9904 times.
✗ Branch 11 not taken.
 9904 imuObsFiltered->p_acceleration = { _kalmanFilter.x(6, 0), _kalmanFilter.x(7, 0), _kalmanFilter.x(8, 0) };
1524
4/8
✓ Branch 1 taken 9904 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9904 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9904 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9904 times.
✗ Branch 11 not taken.
 9904 imuObsFiltered->p_angularRate = { _kalmanFilter.x(0, 0), _kalmanFilter.x(1, 0), _kalmanFilter.x(2, 0) };
1525 }
1526 else // (_imuFusionType == ImuFusionType::BsplineKF)
1527 {
1528
1/2
✓ Branch 1 taken 7482 times.
✗ Branch 2 not taken.
 7482 auto qBsplines = NAV::BsplineKF::quadraticBsplines(_timeSinceStartup, _splineSpacing);
1529
1530 LOG_DATA("{}: timeSinceStartup: {}, qBsplines (stacked B-spline values, cumulatively = 1) = {}, {}, {}", nameId(), _timeSinceStartup, qBsplines.at(0), qBsplines.at(1), qBsplines.at(2));
1531 LOG_DATA("{}: timeSinceStartup: {}, Angular rate B-spline coefficient estimates:\n{}", nameId(), _timeSinceStartup, _kalmanFilter.x.block<9, 1>(0, 0));
1532 LOG_DATA("{}: timeSinceStartup: {}, Acceleration B-spline coefficient estimates:\n{}", nameId(), _timeSinceStartup, _kalmanFilter.x.block<9, 1>(9, 0));
1533
1534 // Estimated angular rate: Cumulative sum of the three estimated B-spline coefficients for the angular rate
1535
3/6
✓ Branch 1 taken 7482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7482 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7482 times.
✗ Branch 8 not taken.
 7482 auto angRateEst = _kalmanFilter.x.block<3, 1>(0, 0) * qBsplines.at(0)
1536
4/8
✓ Branch 1 taken 7482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7482 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7482 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7482 times.
✗ Branch 11 not taken.
 14964 + _kalmanFilter.x.block<3, 1>(3, 0) * qBsplines.at(1)
1537
4/8
✓ Branch 1 taken 7482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7482 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7482 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7482 times.
✗ Branch 11 not taken.
 14964 + _kalmanFilter.x.block<3, 1>(6, 0) * qBsplines.at(2);
1538
1539 // Estimated acceleration: Cumulative sum of the three estimated B-spline coefficients for the acceleration
1540
3/6
✓ Branch 1 taken 7482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7482 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7482 times.
✗ Branch 8 not taken.
 7482 auto accelEst = _kalmanFilter.x.block<3, 1>(9, 0) * qBsplines.at(0)
1541
4/8
✓ Branch 1 taken 7482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7482 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7482 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7482 times.
✗ Branch 11 not taken.
 14964 + _kalmanFilter.x.block<3, 1>(12, 0) * qBsplines.at(1)
1542
4/8
✓ Branch 1 taken 7482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7482 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7482 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7482 times.
✗ Branch 11 not taken.
 14964 + _kalmanFilter.x.block<3, 1>(15, 0) * qBsplines.at(2);
1543
1544 LOG_DATA("{}: imuObs->insTime = {}, timeSinceStartup = {}, angRateEst = {}, accelEst = {}", nameId(), imuObs->insTime.toYMDHMS(), _timeSinceStartup, angRateEst.transpose(), accelEst.transpose());
1545
1546
1/2
✓ Branch 2 taken 7482 times.
✗ Branch 3 not taken.
 7482 imuObsFiltered->p_acceleration = accelEst;
1547
1/2
✓ Branch 2 taken 7482 times.
✗ Branch 3 not taken.
 7482 imuObsFiltered->p_angularRate = angRateEst;
1548 }
1549
1550 // Detect jumps back in time
1551
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 17386 times.
 17386 if (imuObsFiltered->insTime < _lastFiltObs)
1552 {
1553 ✗ LOG_ERROR("{}: imuObsFiltered->insTime < _lastFiltObs --> {}", nameId(), static_cast<double>((imuObsFiltered->insTime - _lastFiltObs).count()));
1554 }
1555 17386 _lastFiltObs = imuObsFiltered->insTime;
1556
1557
1/2
✓ Branch 2 taken 17386 times.
✗ Branch 3 not taken.
 17386 invokeCallbacks(OUTPUT_PORT_INDEX_COMBINED_SIGNAL, imuObsFiltered);
1558
1559
2/2
✓ Branch 0 taken 59640 times.
✓ Branch 1 taken 17386 times.
 77026 for (size_t OUTPUT_PORT_INDEX_BIAS = 1; OUTPUT_PORT_INDEX_BIAS < _nInputPins; ++OUTPUT_PORT_INDEX_BIAS)
1560 {
1561
1/2
✓ Branch 1 taken 59640 times.
✗ Branch 2 not taken.
 59640 auto imuRelativeBiases = std::make_shared<InsGnssLCKFSolution>();
1562 59640 imuRelativeBiases->insTime = imuObs->insTime;
1563 59640 auto biasIndex = _numStatesEst + static_cast<uint8_t>((OUTPUT_PORT_INDEX_BIAS - 1) * _numStatesPerPin);
1564 LOG_DATA("{}: biasIndex = {}", nameId(), biasIndex);
1565
1566
4/8
✓ Branch 1 taken 59640 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 59640 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 59640 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 59640 times.
✗ Branch 11 not taken.
 59640 imuRelativeBiases->b_biasGyro.value = { _kalmanFilter.x(biasIndex, 0), _kalmanFilter.x(biasIndex + 1, 0), _kalmanFilter.x(biasIndex + 2, 0) };
1567
4/8
✓ Branch 1 taken 59640 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 59640 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 59640 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 59640 times.
✗ Branch 11 not taken.
 59640 imuRelativeBiases->b_biasAccel.value = { _kalmanFilter.x(biasIndex + 3, 0), _kalmanFilter.x(biasIndex + 4, 0), _kalmanFilter.x(biasIndex + 5, 0) };
1568
1569 LOG_DATA("{}: timeSinceStartup = {}, Relative bias {}1 Gyro: {}", nameId(), _timeSinceStartup, OUTPUT_PORT_INDEX_BIAS + 1, imuRelativeBiases->b_biasGyro.value.transpose());
1570 LOG_DATA("{}: timeSinceStartup = {}, Relative bias {}1 Accel: {}", nameId(), _timeSinceStartup, OUTPUT_PORT_INDEX_BIAS + 1, imuRelativeBiases->b_biasAccel.value.transpose());
1571
1572
1/2
✓ Branch 2 taken 59640 times.
✗ Branch 3 not taken.
 59640 invokeCallbacks(OUTPUT_PORT_INDEX_BIAS, imuRelativeBiases);
1573 59640 }
1574 17386 }
1575
1576 // -------------------------------------- Measurement noise matrix R -----------------------------------------
1577
1578 ✗ Eigen::MatrixXd NAV::ImuFusion::measurementNoiseMatrix_R_adaptive(double alpha, const Eigen::MatrixXd& R, const Eigen::VectorXd& e, const Eigen::MatrixXd& H, const Eigen::MatrixXd& P)
1579 {
1580 ✗ return alpha * R + (1.0 - alpha) * (e * e.transpose() + H * P * H.transpose());
1581 }
1582
1583 6 void NAV::ImuFusion::measurementNoiseMatrix_R(Eigen::MatrixXd& R, size_t pinIndex) const
1584 {
1585
3/6
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
 6 R.block<3, 3>(0, 0).diagonal() = _measurementNoiseVariances.at(2 * pinIndex);
1586
3/6
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
 6 R.block<3, 3>(3, 3).diagonal() = _measurementNoiseVariances.at(2 * pinIndex + 1);
1587 6 }
1588