0.5.1
Loading...
Searching...
No Matches
ImuFusion.cpp
Go to the documentation of this file.
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
30namespace 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
35static 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
66static void from_json(const json& j, PinData& data) // NOLINT(misc-use-anonymous-namespace)
67{
68 // ------------------------------------------ Initialization ---------------------------------------------
69 if (j.contains("initAngularRateBias"))
70 {
71 j.at("initAngularRateBias").get_to(data.initAngularRateBias);
72 }
73 if (j.contains("initAngularRateBiasUnit"))
74 {
75 j.at("initAngularRateBiasUnit").get_to(data.initAngularRateBiasUnit);
76 }
77 if (j.contains("initAccelerationBias"))
78 {
79 j.at("initAccelerationBias").get_to(data.initAccelerationBias);
80 }
81 if (j.contains("initAccelerationBiasUnit"))
82 {
83 j.at("initAccelerationBiasUnit").get_to(data.initAccelerationBiasUnit);
84 }
85 if (j.contains("initCovarianceAngularRate"))
86 {
87 j.at("initCovarianceAngularRate").get_to(data.initCovarianceAngularRate);
88 }
89 if (j.contains("initCovarianceAngularRateUnit"))
90 {
91 j.at("initCovarianceAngularRateUnit").get_to(data.initCovarianceAngularRateUnit);
92 }
93 if (j.contains("initCovarianceAcceleration"))
94 {
95 j.at("initCovarianceAcceleration").get_to(data.initCovarianceAcceleration);
96 }
97 if (j.contains("initCovarianceAccelerationUnit"))
98 {
99 j.at("initCovarianceAccelerationUnit").get_to(data.initCovarianceAccelerationUnit);
100 }
101 if (j.contains("initCovarianceBiasAngRate"))
102 {
103 j.at("initCovarianceBiasAngRate").get_to(data.initCovarianceBiasAngRate);
104 }
105 if (j.contains("initCovarianceBiasAngRateUnit"))
106 {
107 j.at("initCovarianceBiasAngRateUnit").get_to(data.initCovarianceBiasAngRateUnit);
108 }
109 if (j.contains("initCovarianceBiasAcc"))
110 {
111 j.at("initCovarianceBiasAcc").get_to(data.initCovarianceBiasAcc);
112 }
113 if (j.contains("initCovarianceBiasAccUnit"))
114 {
115 j.at("initCovarianceBiasAccUnit").get_to(data.initCovarianceBiasAccUnit);
116 }
117 // ------------------------------------------- Process Noise ---------------------------------------------
118 if (j.contains("varBiasAccelerationNoise"))
119 {
120 j.at("varBiasAccelerationNoise").get_to(data.varBiasAccelerationNoise);
121 }
122 if (j.contains("varBiasAccelerationNoiseUnit"))
123 {
124 j.at("varBiasAccelerationNoiseUnit").get_to(data.varBiasAccelerationNoiseUnit);
125 }
126 if (j.contains("varBiasAngRateNoise"))
127 {
128 j.at("varBiasAngRateNoise").get_to(data.varBiasAngRateNoise);
129 }
130 if (j.contains("varBiasAngRateNoiseUnit"))
131 {
132 j.at("varBiasAngRateNoiseUnit").get_to(data.varBiasAngRateNoiseUnit);
133 }
134 // ----------------------------------------- Measurement Noise -------------------------------------------
135 if (j.contains("measurementUncertaintyAngularRate"))
136 {
137 j.at("measurementUncertaintyAngularRate").get_to(data.measurementUncertaintyAngularRate);
138 }
139 if (j.contains("measurementUncertaintyAngularRateUnit"))
140 {
141 j.at("measurementUncertaintyAngularRateUnit").get_to(data.measurementUncertaintyAngularRateUnit);
142 }
143 if (j.contains("measurementUncertaintyAcceleration"))
144 {
145 j.at("measurementUncertaintyAcceleration").get_to(data.measurementUncertaintyAcceleration);
146 }
147 if (j.contains("measurementUncertaintyAccelerationUnit"))
148 {
149 j.at("measurementUncertaintyAccelerationUnit").get_to(data.measurementUncertaintyAccelerationUnit);
150 }
151}
152
153/// @brief Write info to a json object
154/// @param[out] j Json output
155/// @param[in] data Object to read info from
156static 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
183static void from_json(const json& j, PinDataIRWKF& data) // NOLINT(misc-use-anonymous-namespace)
184{
185 // ------------------------------------------ Initialization ---------------------------------------------
186 if (j.contains("initAngularRate"))
187 {
188 j.at("initAngularRate").get_to(data.initAngularRate);
189 }
190 if (j.contains("initAngularRateUnit"))
191 {
192 j.at("initAngularRateUnit").get_to(data.initAngularRateUnit);
193 }
194 if (j.contains("initAcceleration"))
195 {
196 j.at("initAcceleration").get_to(data.initAcceleration);
197 }
198 if (j.contains("initAccelerationUnit"))
199 {
200 j.at("initAccelerationUnit").get_to(data.initAccelerationUnit);
201 }
202 if (j.contains("initAngularAcc"))
203 {
204 j.at("initAngularAcc").get_to(data.initAngularAcc);
205 }
206 if (j.contains("initAngularAccUnit"))
207 {
208 j.at("initAngularAccUnit").get_to(data.initAngularAccUnit);
209 }
210 if (j.contains("initJerk"))
211 {
212 j.at("initJerk").get_to(data.initJerk);
213 }
214 if (j.contains("initJerkUnit"))
215 {
216 j.at("initJerkUnit").get_to(data.initJerkUnit);
217 }
218
219 if (j.contains("initCovarianceAngularAcc"))
220 {
221 j.at("initCovarianceAngularAcc").get_to(data.initCovarianceAngularAcc);
222 }
223 if (j.contains("initCovarianceAngularAccUnit"))
224 {
225 j.at("initCovarianceAngularAccUnit").get_to(data.initCovarianceAngularAccUnit);
226 }
227 if (j.contains("initCovarianceJerk"))
228 {
229 j.at("initCovarianceJerk").get_to(data.initCovarianceJerk);
230 }
231 if (j.contains("initCovarianceJerkUnit"))
232 {
233 j.at("initCovarianceJerkUnit").get_to(data.initCovarianceJerkUnit);
234 }
235
236 // ------------------------------------------- Process Noise ---------------------------------------------
237 if (j.contains("varAngularAccNoise"))
238 {
239 j.at("varAngularAccNoise").get_to(data.varAngularAccNoise);
240 }
241 if (j.contains("varAngularAccNoiseUnit"))
242 {
243 j.at("varAngularAccNoiseUnit").get_to(data.varAngularAccNoiseUnit);
244 }
245 if (j.contains("varJerkNoise"))
246 {
247 j.at("varJerkNoise").get_to(data.varJerkNoise);
248 }
249 if (j.contains("varJerkNoiseUnit"))
250 {
251 j.at("varJerkNoiseUnit").get_to(data.varJerkNoiseUnit);
252 }
253}
254
255/// @brief Write info to a json object
256/// @param[out] j Json output
257/// @param[in] data Object to read info from
258static 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
281static void from_json(const json& j, PinDataBsplineKF& data) // NOLINT(misc-use-anonymous-namespace)
282{
283 // ------------------------------------------ Initialization ---------------------------------------------
284
285 if (j.contains("initCoeffsAngRate"))
286 {
287 j.at("initCoeffsAngRate").get_to(data.initCoeffsAngRate);
288 }
289 if (j.contains("initCoeffsAngularRateUnit"))
290 {
291 j.at("initCoeffsAngularRateUnit").get_to(data.initCoeffsAngularRateUnit);
292 }
293 if (j.contains("initCoeffsAccel"))
294 {
295 j.at("initCoeffsAccel").get_to(data.initCoeffsAccel);
296 }
297 if (j.contains("initCoeffsAccelUnit"))
298 {
299 j.at("initCoeffsAccelUnit").get_to(data.initCoeffsAccelUnit);
300 }
301 if (j.contains("initCovarianceCoeffsAngRate"))
302 {
303 j.at("initCovarianceCoeffsAngRate").get_to(data.initCovarianceCoeffsAngRate);
304 }
305 if (j.contains("initCovarianceCoeffsAngRateUnit"))
306 {
307 j.at("initCovarianceCoeffsAngRateUnit").get_to(data.initCovarianceCoeffsAngRateUnit);
308 }
309 if (j.contains("initCovarianceCoeffsAccel"))
310 {
311 j.at("initCovarianceCoeffsAccel").get_to(data.initCovarianceCoeffsAccel);
312 }
313 if (j.contains("initCovarianceCoeffsAccelUnit"))
314 {
315 j.at("initCovarianceCoeffsAccelUnit").get_to(data.initCovarianceCoeffsAccelUnit);
316 }
317
318 // ------------------------------------------- Process Noise ---------------------------------------------
319
320 if (j.contains("varCoeffsAngRateNoise"))
321 {
322 j.at("varCoeffsAngRateNoise").get_to(data.varCoeffsAngRateNoise);
323 }
324 if (j.contains("varCoeffsAngRateUnit"))
325 {
326 j.at("varCoeffsAngRateUnit").get_to(data.varCoeffsAngRateUnit);
327 }
328 if (j.contains("varCoeffsAccelNoise"))
329 {
330 j.at("varCoeffsAccelNoise").get_to(data.varCoeffsAccelNoise);
331 }
332 if (j.contains("varCoeffsAccelUnit"))
333 {
334 j.at("varCoeffsAccelUnit").get_to(data.varCoeffsAccelUnit);
335 }
336}
337
338} // namespace NAV
339
340NAV::ImuFusion::ImuFusion()
341 : Imu(typeStatic())
342{
343 LOG_TRACE("{}: called", name);
344
345 _hasConfig = true;
346 _guiConfigDefaultWindowSize = { 991, 1059 };
347
348 CreateOutputPin("Combined ImuObs", Pin::Type::Flow, { NAV::ImuObs::type() });
349 updateNumberOfInputPins();
350}
351
352NAV::ImuFusion::~ImuFusion()
353{
354 LOG_TRACE("{}: called", nameId());
355}
356
357std::string NAV::ImuFusion::typeStatic()
358{
359 return "ImuFusion";
360}
361
362std::string NAV::ImuFusion::type() const
363{
364 return typeStatic();
365}
366
367std::string NAV::ImuFusion::category()
368{
369 return "Data Processor";
370}
371
372void 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
1110void NAV::ImuFusion::restore(json const& j)
1111{
1112 LOG_TRACE("{}: called", nameId());
1113
1114 if (j.contains("imuFusionType"))
1115 {
1116 j.at("imuFusionType").get_to(_imuFusionType);
1117 }
1118 if (j.contains("checkKalmanMatricesRanks"))
1119 {
1120 j.at("checkKalmanMatricesRanks").get_to(_checkKalmanMatricesRanks);
1121 }
1122 if (j.contains("nInputPins"))
1123 {
1124 j.at("nInputPins").get_to(_nInputPins);
1125 updateNumberOfInputPins();
1126 }
1127 if (j.contains("imuFrequency"))
1128 {
1129 j.at("imuFrequency").get_to(_imuFrequency);
1130 }
1131 if (j.contains("numStates"))
1132 {
1133 j.at("numStates").get_to(_numStates);
1134 }
1135 if (j.contains("pinData"))
1136 {
1137 j.at("pinData").get_to(_pinData);
1138 }
1139 if (j.contains("pinDataIRWKF"))
1140 {
1141 j.at("pinDataIRWKF").get_to(_pinDataIRWKF);
1142 }
1143 if (j.contains("pinDataBsplineKF"))
1144 {
1145 j.at("pinDataBsplineKF").get_to(_pinDataBsplineKF);
1146 }
1147 if (j.contains("initCoeffsAngRateTemp"))
1148 {
1149 j.at("initCoeffsAngRateTemp").get_to(_initCoeffsAngRateTemp);
1150 }
1151 if (j.contains("initCoeffsAccelTemp"))
1152 {
1153 j.at("initCoeffsAccelTemp").get_to(_initCoeffsAccelTemp);
1154 }
1155 if (j.contains("initCovarianceCoeffsAngRateTemp"))
1156 {
1157 j.at("initCovarianceCoeffsAngRateTemp").get_to(_initCovarianceCoeffsAngRateTemp);
1158 }
1159 if (j.contains("initCovarianceCoeffsAccelTemp"))
1160 {
1161 j.at("initCovarianceCoeffsAccelTemp").get_to(_initCovarianceCoeffsAccelTemp);
1162 }
1163 if (j.contains("procNoiseCoeffsAngRateTemp"))
1164 {
1165 j.at("procNoiseCoeffsAngRateTemp").get_to(_procNoiseCoeffsAngRateTemp);
1166 }
1167 if (j.contains("procNoiseCoeffsAccelTemp"))
1168 {
1169 j.at("procNoiseCoeffsAccelTemp").get_to(_procNoiseCoeffsAccelTemp);
1170 }
1171 if (j.contains("autoInitKF"))
1172 {
1173 j.at("autoInitKF").get_to(_autoInitKF);
1174 }
1175 if (j.contains("initJerkAngAcc"))
1176 {
1177 j.at("initJerkAngAcc").get_to(_initJerkAngAcc);
1178 }
1179 if (j.contains("_kfInitialized"))
1180 {
1181 j.at("_kfInitialized").get_to(_kfInitialized);
1182 }
1183 if (j.contains("averageEndTime"))
1184 {
1185 j.at("averageEndTime").get_to(_averageEndTime);
1186 }
1187 if (j.contains("splineSpacing"))
1188 {
1189 j.at("splineSpacing").get_to(_splineSpacing);
1190 }
1191}
1192
1193bool NAV::ImuFusion::initialize()
1194{
1195 LOG_TRACE("{}: called", nameId());
1196
1197 _imuRotations_accel.clear();
1198 _imuRotations_gyro.clear();
1199 _biasCovariances.clear();
1200 _processNoiseVariances.clear();
1201 _measurementNoiseVariances.clear();
1202
1203 _cumulatedImuObs.clear();
1204 _cumulatedPinIds.clear();
1205 _lastFiltObs.reset();
1206 _latestTimestamp = InsTime{};
1207 _firstTimestamp.reset();
1208
1209 _imuPosSet = false;
1210 _kfInitialized = false;
1211
1212 for (size_t pinIndex = 0; pinIndex < _pinData.size(); pinIndex++)
1213 {
1214 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 _numStatesEst = _imuFusionType == ImuFusionType::IRWKF ? _numStatesEstIRWKF : _numStatesEstBsplineKF;
1221
1222 _numStates = _numStatesEst + static_cast<uint8_t>((_nInputPins - 1) * _numStatesPerPin);
1223
1224 _kalmanFilter = KalmanFilter{ _numStates, _numMeasurements };
1225 _kalmanFilter.setZero();
1226
1227 initializeMountingAngles();
1228
1229 // --------------------------------------------------------- KF Initializations ------------------------------------------------------------
1230 if (!_autoInitKF) // i.e. manual initialization thru inputs from the GUI
1231 {
1232 auto dtInit = 1.0 / _imuFrequency; // Initial state transition time in [s]
1233
1234 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 _kalmanFilter = BsplineKF::initializeKalmanFilterManually(_nInputPins, _pinData, _pinDataBsplineKF, _numStates, dtInit, _processNoiseVariances, _kalmanFilter, _imuCharacteristicsIdentical, _imuBiasesIdentical);
1241 _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 _measurementNoiseVariances.resize(2 * _nInputPins);
1252
1253 size_t pinDataIdx = 0;
1254 for (size_t pinIndex = 0; pinIndex < _nInputPins; ++pinIndex)
1255 {
1256 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 switch (_pinData[pinDataIdx].measurementUncertaintyAngularRateUnit)
1263 {
1264 case PinData::AngRateVarianceUnit::rad_s:
1265 _measurementNoiseVariances[2 * pinIndex] = (_pinData[pinDataIdx].measurementUncertaintyAngularRate).array().pow(2);
1266 break;
1267 case PinData::AngRateVarianceUnit::deg_s:
1268 _measurementNoiseVariances[2 * pinIndex] = (deg2rad(_pinData[pinDataIdx].measurementUncertaintyAngularRate)).array().pow(2);
1269 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 switch (_pinData[pinDataIdx].measurementUncertaintyAccelerationUnit)
1280 {
1281 case PinData::AccelerationVarianceUnit::m2_s4:
1282 _measurementNoiseVariances[1 + 2 * pinIndex] = _pinData[pinDataIdx].measurementUncertaintyAcceleration;
1283 break;
1284 case PinData::AccelerationVarianceUnit::m_s2:
1285 _measurementNoiseVariances[1 + 2 * pinIndex] = (_pinData[pinDataIdx].measurementUncertaintyAcceleration).array().pow(2);
1286 break;
1287 }
1288 }
1289
1290 if (_imuCharacteristicsIdentical)
1291 {
1292 measurementNoiseMatrix_R(_kalmanFilter.R);
1293 LOG_DATA("{}: imuCharacteristicsIdentical - kalmanFilter.R =\n{}", nameId(), _kalmanFilter.R);
1294 }
1295
1296 if (!_autoInitKF) { _kfInitialized = true; } // Auto-init initializes KF at 'avgEndTime' seconds --> see 'recvSignal'
1297
1298 LOG_DEBUG("ImuFusion initialized");
1299
1300 return true;
1301}
1302
1303void NAV::ImuFusion::deinitialize()
1304{
1305 LOG_TRACE("{}: called", nameId());
1306}
1307
1308void NAV::ImuFusion::updateNumberOfInputPins()
1309{
1310 while (inputPins.size() < _nInputPins)
1311 {
1312 CreateInputPin(fmt::format("Pin {}", inputPins.size() + 1).c_str(), Pin::Type::Flow,
1313 { NAV::ImuObs::type() }, &ImuFusion::recvSignal);
1314 _pinData.emplace_back();
1315 if (outputPins.size() < _nInputPins)
1316 {
1317 CreateOutputPin(fmt::format("ImuBiases {}1", outputPins.size() + 1).c_str(), Pin::Type::Flow, { NAV::InsGnssLCKFSolution::type() });
1318 }
1319 }
1320 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 _pinData.resize(_nInputPins);
1327 initializeMountingAngles();
1328}
1329
1330void NAV::ImuFusion::initializeMountingAngles()
1331{
1332 _imuRotations_accel.resize(_nInputPins);
1333 _imuRotations_gyro.resize(_nInputPins);
1334 for (size_t i = 0; i < _nInputPins; ++i)
1335 {
1336 _imuRotations_accel[i] = Eigen::Matrix3d::Zero();
1337 _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 _imuRotations_accel[i](0, 0) = std::nan("");
1341 _imuRotations_gyro[i](0, 0) = std::nan("");
1342 }
1343}
1344
1345void NAV::ImuFusion::recvSignal(NAV::InputPin::NodeDataQueue& queue, size_t pinIdx)
1346{
1347 auto imuObs = std::static_pointer_cast<const ImuObs>(queue.extract_front());
1348
1349 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 if (_latestTimestamp.empty())
1356 {
1357 // Initial time step for KF prediction
1358 InsTime dt_init = InsTime{ 0, 0, 0, 0, 0, 1.0 / _imuFrequency };
1359 _latestTimestamp = InsTime{ 0, 0, 0, 0, 0, (imuObs->insTime - dt_init).count() };
1360 _firstTimestamp = imuObs->insTime;
1361
1362 // Time until averaging ends and filtering starts (for auto-init of KF)
1363 _avgEndTime = imuObs->insTime + std::chrono::milliseconds(static_cast<int>(1e3 * _averageEndTime));
1364 }
1365
1366 _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 auto dt = static_cast<double>((imuObs->insTime - _latestTimestamp).count());
1371 _latestTimestamp = imuObs->insTime;
1372 LOG_DATA("{}: dt = {}", nameId(), dt);
1373
1374 if (_kfInitialized)
1375 {
1376 if (_imuFusionType == ImuFusionType::IRWKF)
1377 {
1378 IRWKF::stateTransitionMatrix_Phi(_kalmanFilter.Phi, dt);
1379 IRWKF::processNoiseMatrix_Q(_kalmanFilter.Q, dt, _processNoiseVariances, _numStates);
1380 }
1381 else // (_imuFusionType == ImuFusionType::Bspline)
1382 {
1383 BsplineKF::processNoiseMatrix_Q(_kalmanFilter.Q, dt, _processNoiseVariances, _numStates);
1384
1385 if (_timeSinceStartup >= _latestKnot)
1386 {
1387 _latestKnot += _splineSpacing;
1388
1389 BsplineKF::rotateCoeffStates(_kalmanFilter.x);
1390 LOG_DATA("{}: kalmanFilter.P before B-spline coeff rotation =\n{}", nameId(), _kalmanFilter.P.block<18, 18>(0, 0));
1391 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 if (_checkKalmanMatricesRanks)
1400 {
1401 if (inputPins.at(_pinData.size() - 1).isPinLinked())
1402 {
1403 auto rank = _kalmanFilter.P.fullPivLu().rank();
1404 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 if (std::isnan(_imuRotations_accel[pinIdx](0, 0)))
1414 {
1415 // Rotation matrix of the accelerometer platform to body frame
1416 _imuRotations_accel[pinIdx] = imuObs->imuPos.b_quat_p().toRotationMatrix();
1417 }
1418 if (std::isnan(_imuRotations_gyro[pinIdx](0, 0)))
1419 {
1420 // Rotation matrix of the gyro platform to body frame
1421 _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 auto DCM_accel = _imuRotations_accel.at(pinIdx);
1426 LOG_DATA("DCM_accel =\n{}", DCM_accel);
1427 auto DCM_gyro = _imuRotations_gyro.at(pinIdx);
1428 LOG_DATA("{}: DCM_gyro =\n{}", nameId(), DCM_gyro);
1429
1430 if (_imuFusionType == ImuFusionType::IRWKF)
1431 {
1432 _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 _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 if (!_imuCharacteristicsIdentical)
1442 {
1443 measurementNoiseMatrix_R(_kalmanFilter.R, pinIdx);
1444 LOG_DATA("{}: kalmanFilter.R =\n{}", nameId(), _kalmanFilter.R);
1445 }
1446
1447 if (_checkKalmanMatricesRanks)
1448 {
1449 auto rank = (_kalmanFilter.H * _kalmanFilter.P * _kalmanFilter.H.transpose() + _kalmanFilter.R).fullPivLu().rank();
1450 if (rank != _kalmanFilter.H.rows())
1451 {
1452 LOG_WARN("{}: (HPH^T + R).rank = {}", nameId(), rank);
1453 }
1454 }
1455
1456 if (_autoInitKF && !_kfInitialized)
1457 {
1458 if (imuObs->insTime < _avgEndTime)
1459 {
1460 _cumulatedImuObs.push_back(imuObs);
1461 _cumulatedPinIds.push_back(pinIdx);
1462 }
1463 else // if (imuObs->insTime == _avgEndTime) // <-- do auto-init, once _avgEndTime is reached
1464 {
1465 double dtInit = 1.0 / _imuFrequency;
1466 _kalmanFilter = IRWKF::initializeKalmanFilterAuto(_nInputPins, _pinData, _pinDataIRWKF, _cumulatedPinIds, _cumulatedImuObs, _initJerkAngAcc, dtInit, _numStates, _numMeasurements, _processNoiseVariances, _kalmanFilter);
1467 _kfInitialized = true; // Start Kalman Filter
1468 }
1469 }
1470 if (_kfInitialized)
1471 {
1472 combineSignals(imuObs);
1473 }
1474}
1475
1476void NAV::ImuFusion::combineSignals(const std::shared_ptr<const ImuObs>& imuObs)
1477{
1478 LOG_DATA("{}: called", nameId());
1479
1480 auto imuObsFiltered = std::make_shared<ImuObs>(this->_imuPos);
1481
1482 LOG_DATA("{}: Estimated state before prediction: x =\n{}", nameId(), _kalmanFilter.x);
1483
1484 _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 _kalmanFilter.z.block<3, 1>(0, 0) = imuObs->p_angularRate;
1491 _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 _kalmanFilter.correct();
1498 LOG_DATA("{}: Estimated state after correction: x =\n{}", nameId(), _kalmanFilter.x);
1499
1500 if (_checkKalmanMatricesRanks)
1501 {
1502 auto rankH = (_kalmanFilter.H * _kalmanFilter.P * _kalmanFilter.H.transpose() + _kalmanFilter.R).fullPivLu().rank();
1503 if (rankH != _kalmanFilter.H.rows())
1504 {
1505 LOG_WARN("{}: (HPH^T + R).rank = {}", nameId(), rankH);
1506 }
1507
1508 if (inputPins.at(_pinData.size() - 1).isPinLinked())
1509 {
1510 auto rankP = _kalmanFilter.P.fullPivLu().rank();
1511 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 imuObsFiltered->insTime = imuObs->insTime;
1521 if (_imuFusionType == ImuFusionType::IRWKF)
1522 {
1523 imuObsFiltered->p_acceleration = { _kalmanFilter.x(6, 0), _kalmanFilter.x(7, 0), _kalmanFilter.x(8, 0) };
1524 imuObsFiltered->p_angularRate = { _kalmanFilter.x(0, 0), _kalmanFilter.x(1, 0), _kalmanFilter.x(2, 0) };
1525 }
1526 else // (_imuFusionType == ImuFusionType::BsplineKF)
1527 {
1528 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 auto angRateEst = _kalmanFilter.x.block<3, 1>(0, 0) * qBsplines.at(0)
1536 + _kalmanFilter.x.block<3, 1>(3, 0) * qBsplines.at(1)
1537 + _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 auto accelEst = _kalmanFilter.x.block<3, 1>(9, 0) * qBsplines.at(0)
1541 + _kalmanFilter.x.block<3, 1>(12, 0) * qBsplines.at(1)
1542 + _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 imuObsFiltered->p_acceleration = accelEst;
1547 imuObsFiltered->p_angularRate = angRateEst;
1548 }
1549
1550 // Detect jumps back in time
1551 if (imuObsFiltered->insTime < _lastFiltObs)
1552 {
1553 LOG_ERROR("{}: imuObsFiltered->insTime < _lastFiltObs --> {}", nameId(), static_cast<double>((imuObsFiltered->insTime - _lastFiltObs).count()));
1554 }
1555 _lastFiltObs = imuObsFiltered->insTime;
1556
1557 invokeCallbacks(OUTPUT_PORT_INDEX_COMBINED_SIGNAL, imuObsFiltered);
1558
1559 for (size_t OUTPUT_PORT_INDEX_BIAS = 1; OUTPUT_PORT_INDEX_BIAS < _nInputPins; ++OUTPUT_PORT_INDEX_BIAS)
1560 {
1561 auto imuRelativeBiases = std::make_shared<InsGnssLCKFSolution>();
1562 imuRelativeBiases->insTime = imuObs->insTime;
1563 auto biasIndex = _numStatesEst + static_cast<uint8_t>((OUTPUT_PORT_INDEX_BIAS - 1) * _numStatesPerPin);
1564 LOG_DATA("{}: biasIndex = {}", nameId(), biasIndex);
1565
1566 imuRelativeBiases->b_biasGyro.value = { _kalmanFilter.x(biasIndex, 0), _kalmanFilter.x(biasIndex + 1, 0), _kalmanFilter.x(biasIndex + 2, 0) };
1567 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 invokeCallbacks(OUTPUT_PORT_INDEX_BIAS, imuRelativeBiases);
1573 }
1574}
1575
1576// -------------------------------------- Measurement noise matrix R -----------------------------------------
1577
1578Eigen::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
1583void NAV::ImuFusion::measurementNoiseMatrix_R(Eigen::MatrixXd& R, size_t pinIndex) const
1584{
1585 R.block<3, 3>(0, 0).diagonal() = _measurementNoiseVariances.at(2 * pinIndex);
1586 R.block<3, 3>(3, 3).diagonal() = _measurementNoiseVariances.at(2 * pinIndex + 1);
1587}
BsplineKF.hpp
Kalman filter matrices for the B-spline Multi-IMU fusion.
EnumCombo.hpp
Combo representing an enumeration.
FlowManager.hpp
Save/Load the Nodes.
json
nlohmann::json json
json namespace
Definition FlowManager.hpp:23
HelpMarker.hpp
Text Help Marker (?) with Tooltip.
IRWKF.hpp
Kalman filter matrices for the Integrated-Random-Walk (IRW) Multi-IMU fusion.
ImuFusion.hpp
Combines signals of sensors that provide the same signal-type to one signal.
InputWithUnit.hpp
Defines Widgets which allow the input of values and the selection of the unit.
InsGnssLCKFSolution.hpp
Loosely-coupled Kalman Filter INS/GNSS errors.
Json.hpp
Defines how to save certain datatypes to json.
Logger.hpp
Utility class for logging to console and file.
LOG_DEBUG
#define LOG_DEBUG
Debug information. Should not be called on functions which receive observations (spamming)
Definition Logger.hpp:67
LOG_DATA
#define LOG_DATA
All output which occurs repeatedly every time observations are received.
Definition Logger.hpp:29
LOG_ERROR
#define LOG_ERROR
Error occurred, which stops part of the program to work, but not everything.
Definition Logger.hpp:73
LOG_WARN
#define LOG_WARN
Error occurred, but a fallback option exists and program continues to work normally.
Definition Logger.hpp:71
LOG_INFO
#define LOG_INFO
Info to the user on the state of the program.
Definition Logger.hpp:69
LOG_TRACE
#define LOG_TRACE
Detailled info to trace the execution of the program. Should not be called on functions which receive...
Definition Logger.hpp:65
QuadraticBsplines.hpp
Constructs four overlapping qaudratic B-splines.
NAV::ImuFusion::_kalmanFilter
KalmanFilter _kalmanFilter
Kalman Filter representation.
Definition ImuFusion.hpp:141
NAV::ImuFusion::updateNumberOfInputPins
void updateNumberOfInputPins()
Adds/Deletes Input Pins depending on the variable _nInputPins.
Definition ImuFusion.cpp:1308
NAV::ImuFusion::_imuRotations_accel
std::vector< Eigen::Matrix3d > _imuRotations_accel
Rotations of all connected accelerometers - key: pinIndex, value: Rotation matrix of the acceleromete...
Definition ImuFusion.hpp:203
NAV::ImuFusion::_numStatesEst
uint8_t _numStatesEst
Number of estimated states (depends on imuFusionType)
Definition ImuFusion.hpp:135
NAV::ImuFusion::_numStatesEstIRWKF
const uint8_t _numStatesEstIRWKF
Number of states estimated by the IRW-KF (angular rate, angular acceleration, specific force,...
Definition ImuFusion.hpp:120
NAV::ImuFusion::initializeMountingAngles
void initializeMountingAngles()
Initializes the rotation matrices used for the mounting angles of the sensors.
Definition ImuFusion.cpp:1330
NAV::ImuFusion::type
std::string type() const override
String representation of the Class Type.
Definition ImuFusion.cpp:362
NAV::ImuFusion::initialize
bool initialize() override
Initialize the node.
Definition ImuFusion.cpp:1193
NAV::ImuFusion::_procNoiseCoeffsAccelTemp
Eigen::Vector3d _procNoiseCoeffsAccelTemp
Temporary vector for the initial coefficients' process noise for the acceleration.
Definition ImuFusion.hpp:193
NAV::ImuFusion::_kfInitialized
bool _kfInitialized
flag to check whether KF has been auto-initialized
Definition ImuFusion.hpp:172
NAV::ImuFusion::_checkKalmanMatricesRanks
bool _checkKalmanMatricesRanks
Check the rank of the Kalman matrices every iteration (computationally expensive)
Definition ImuFusion.hpp:226
NAV::ImuFusion::_imuCharacteristicsIdentical
bool _imuCharacteristicsIdentical
If the multiple IMUs have the same characteristics, GUI input cells can be reduced considerably.
Definition ImuFusion.hpp:157
NAV::ImuFusion::_imuRotations_gyro
std::vector< Eigen::Matrix3d > _imuRotations_gyro
Rotations of all connected gyros - key: pinIndex, value: Rotation matrix of the gyro platform to body...
Definition ImuFusion.hpp:206
NAV::ImuFusion::_initCovarianceCoeffsAngRateTemp
Eigen::Vector3d _initCovarianceCoeffsAngRateTemp
Temporary vector for the initial coefficients' initial covariance for the angular rate.
Definition ImuFusion.hpp:187
NAV::ImuFusion::_measurementNoiseVariances
std::vector< Eigen::Vector3d > _measurementNoiseVariances
Container for measurement noises of each sensor.
Definition ImuFusion.hpp:200
NAV::ImuFusion::_numBsplines
const uint8_t _numBsplines
Number of quadratic B-splines that make up the entire 3D stacked B-spline.
Definition ImuFusion.hpp:126
NAV::ImuFusion::save
json save() const override
Saves the node into a json object.
Definition ImuFusion.cpp:1081
NAV::ImuFusion::recvSignal
void recvSignal(InputPin::NodeDataQueue &queue, size_t pinIdx)
Receive Function for the signal at the time tₖ
Definition ImuFusion.cpp:1345
NAV::ImuFusion::_numMeasurements
const uint8_t _numMeasurements
Number of measurements overall.
Definition ImuFusion.hpp:132
NAV::ImuFusion::_nInputPins
size_t _nInputPins
Number of input pins.
Definition ImuFusion.hpp:117
NAV::ImuFusion::_lastFiltObs
InsTime _lastFiltObs
Previous observation (for timestamp)
Definition ImuFusion.hpp:104
NAV::ImuFusion::~ImuFusion
~ImuFusion() override
Destructor.
Definition ImuFusion.cpp:352
NAV::ImuFusion::measurementNoiseMatrix_R
void measurementNoiseMatrix_R(Eigen::MatrixXd &R, size_t pinIndex=0) const
Calculates the initial measurement noise matrix R.
Definition ImuFusion.cpp:1583
NAV::ImuFusion::_imuPos
ImuPos _imuPos
Position and rotation information for conversion from platform to body frame.
Definition ImuFusion.hpp:66
NAV::ImuFusion::_firstTimestamp
InsTime _firstTimestamp
Saves the first timestamp in [s].
Definition ImuFusion.hpp:213
NAV::ImuFusion::_pinDataIRWKF
PinDataIRWKF _pinDataIRWKF
Stores IRW-KF specific parameter data.
Definition ImuFusion.hpp:178
NAV::ImuFusion::_initCoeffsAccelTemp
Eigen::Vector3d _initCoeffsAccelTemp
Temporary vector for the initial coefficients for acceleration.
Definition ImuFusion.hpp:185
NAV::ImuFusion::_imuFrequency
double _imuFrequency
Highest IMU sample rate in [Hz] (for time step in KF prediction)
Definition ImuFusion.hpp:145
NAV::ImuFusion::_biasCovariances
std::vector< Eigen::Vector3d > _biasCovariances
Container for the bias covariances.
Definition ImuFusion.hpp:196
NAV::ImuFusion::_averageEndTime
double _averageEndTime
Time until averaging ends and filtering starts in [s].
Definition ImuFusion.hpp:148
NAV::ImuFusion::typeStatic
static std::string typeStatic()
String representation of the Class Type.
Definition ImuFusion.cpp:357
NAV::ImuFusion::_imuPosSet
bool _imuPosSet
Check whether the combined solution has an '_imuPos' set.
Definition ImuFusion.hpp:229
NAV::ImuFusion::measurementNoiseMatrix_R_adaptive
static Eigen::MatrixXd measurementNoiseMatrix_R_adaptive(double alpha, const Eigen::MatrixXd &R, const Eigen::VectorXd &e, const Eigen::MatrixXd &H, const Eigen::MatrixXd &P)
Calculates the adaptive measurement noise matrix R.
Definition ImuFusion.cpp:1578
NAV::ImuFusion::OUTPUT_PORT_INDEX_COMBINED_SIGNAL
static constexpr size_t OUTPUT_PORT_INDEX_COMBINED_SIGNAL
Flow (ImuObs)
Definition ImuFusion.hpp:69
NAV::ImuFusion::guiConfig
void guiConfig() override
ImGui config window which is shown on double click.
Definition ImuFusion.cpp:372
NAV::ImuFusion::ImuFusion
ImuFusion()
Default constructor.
Definition ImuFusion.cpp:340
NAV::ImuFusion::_cumulatedImuObs
std::vector< std::shared_ptr< const NAV::ImuObs > > _cumulatedImuObs
Container that collects all imuObs for averaging for auto-init of the KF.
Definition ImuFusion.hpp:163
NAV::ImuFusion::restore
void restore(const json &j) override
Restores the node from a json object.
Definition ImuFusion.cpp:1110
NAV::ImuFusion::category
static std::string category()
String representation of the Class Category.
Definition ImuFusion.cpp:367
NAV::ImuFusion::_initCovarianceCoeffsAccelTemp
Eigen::Vector3d _initCovarianceCoeffsAccelTemp
Temporary vector for the initial coefficients' initial covariance for the acceleration.
Definition ImuFusion.hpp:189
NAV::ImuFusion::_cumulatedPinIds
std::vector< size_t > _cumulatedPinIds
Container that collects all pinIds for averaging for auto-init of the KF.
Definition ImuFusion.hpp:166
NAV::ImuFusion::_numStates
uint8_t _numStates
Number of states overall.
Definition ImuFusion.hpp:138
NAV::ImuFusion::_numStatesEstBsplineKF
const uint8_t _numStatesEstBsplineKF
Number of states estimated by the B-spline KF (3 stacked B-splines in 3D for angular rate and specifi...
Definition ImuFusion.hpp:123
NAV::ImuFusion::_timeSinceStartup
double _timeSinceStartup
Time since startup in [s].
Definition ImuFusion.hpp:216
NAV::ImuFusion::_initJerkAngAcc
bool _initJerkAngAcc
flag to determine how jerk and angular acceleration states are initialized if '_autoInitKF = true'
Definition ImuFusion.hpp:169
NAV::ImuFusion::_processNoiseVariances
std::vector< Eigen::VectorXd > _processNoiseVariances
Container for process noise of each state.
Definition ImuFusion.hpp:198
NAV::ImuFusion::deinitialize
void deinitialize() override
Deinitialize the node.
Definition ImuFusion.cpp:1303
NAV::ImuFusion::_latestKnot
double _latestKnot
Latest knot in [s].
Definition ImuFusion.hpp:219
NAV::ImuFusion::_numStatesPerPin
const uint8_t _numStatesPerPin
Number of states per pin (biases of accel and gyro)
Definition ImuFusion.hpp:129
NAV::ImuFusion::_imuFusionType
ImuFusionType _imuFusionType
KF-type for the IMU fusion, selected in the GUI.
Definition ImuFusion.hpp:244
NAV::ImuFusion::_autoInitKF
bool _autoInitKF
Auto-initialize the Kalman Filter - GUI setting.
Definition ImuFusion.hpp:154
NAV::ImuFusion::ImuFusionType::Bspline
@ Bspline
B-spline Kalman filter.
Definition ImuFusion.hpp:235
NAV::ImuFusion::ImuFusionType::IRWKF
@ IRWKF
IRW Kalman filter.
Definition ImuFusion.hpp:234
NAV::ImuFusion::_splineSpacing
double _splineSpacing
Time difference between two quadratic B-splines in the stacked B-spline.
Definition ImuFusion.hpp:222
NAV::ImuFusion::_pinData
std::vector< PinData > _pinData
Stores parameter data for each connected sensor.
Definition ImuFusion.hpp:176
NAV::ImuFusion::_procNoiseCoeffsAngRateTemp
Eigen::Vector3d _procNoiseCoeffsAngRateTemp
Temporary vector for the initial coefficients' process noise for the angular rate.
Definition ImuFusion.hpp:191
NAV::ImuFusion::combineSignals
void combineSignals(const std::shared_ptr< const ImuObs > &imuObs)
Combines the signals.
Definition ImuFusion.cpp:1476
NAV::ImuFusion::_pinDataBsplineKF
PinDataBsplineKF _pinDataBsplineKF
Stores Bspline-KF specific parameter data.
Definition ImuFusion.hpp:180
NAV::ImuFusion::_imuBiasesIdentical
bool _imuBiasesIdentical
If the multiple IMUs have the same bias, GUI input cells can be reduced considerably.
Definition ImuFusion.hpp:160
NAV::ImuFusion::_latestTimestamp
InsTime _latestTimestamp
Saves the timestamp of the measurement before in [s].
Definition ImuFusion.hpp:210
NAV::ImuFusion::_avgEndTime
InsTime _avgEndTime
Time until averaging ends and filtering starts as 'InsTime'.
Definition ImuFusion.hpp:151
NAV::ImuFusion::_initCoeffsAngRateTemp
Eigen::Vector3d _initCoeffsAngRateTemp
Temporary vector for the initial coefficients for angular rate.
Definition ImuFusion.hpp:183
NAV::ImuObs::type
static std::string type()
Returns the type of the data class.
Definition ImuObs.hpp:33
NAV::Imu::Imu
Imu(const Imu &)=delete
Copy constructor.
NAV::InputPin::NodeDataQueue
TsDeque< std::shared_ptr< const NAV::NodeData > > NodeDataQueue
Node data queue type.
Definition Pin.hpp:707
NAV::InsTime
The class is responsible for all time-related tasks.
Definition InsTime.hpp:710
NAV::KalmanFilter
Generalized Kalman Filter class.
Definition KalmanFilter.hpp:25
NAV::Node::doDeinitialize
bool doDeinitialize(bool wait=false)
Asks the node worker to deinitialize the node.
Definition Node.cpp:465
NAV::Node::_guiConfigDefaultWindowSize
ImVec2 _guiConfigDefaultWindowSize
Definition Node.hpp:522
NAV::Node::outputPins
std::vector< OutputPin > outputPins
List of output pins.
Definition Node.hpp:511
NAV::Node::DeleteOutputPin
bool DeleteOutputPin(size_t pinIndex)
Deletes the output pin. Invalidates the pin reference given.
Definition Node.cpp:298
NAV::Node::inputPins
std::vector< InputPin > inputPins
List of input pins.
Definition Node.hpp:509
NAV::Node::CreateOutputPin
OutputPin * CreateOutputPin(const char *name, Pin::Type pinType, const std::vector< std::string > &dataIdentifier, OutputPin::PinData data=static_cast< void * >(nullptr), int idx=-1)
Create an Output Pin object.
Definition Node.cpp:278
NAV::Node::nameId
std::string nameId() const
Node name and id.
Definition Node.cpp:323
NAV::Node::name
std::string name
Name of the Node.
Definition Node.hpp:507
NAV::Node::CreateInputPin
InputPin * CreateInputPin(const char *name, Pin::Type pinType, const std::vector< std::string > &dataIdentifier={}, InputPin::Callback callback=static_cast< InputPin::FlowFirableCallbackFunc >(nullptr), InputPin::FlowFirableCheckFunc firable=nullptr, int priority=0, int idx=-1)
Create an Input Pin object.
Definition Node.cpp:252
NAV::Node::invokeCallbacks
void invokeCallbacks(size_t portIndex, const std::shared_ptr< const NodeData > &data)
Calls all registered callbacks on the specified output port.
Definition Node.cpp:179
NAV::Node::DeleteInputPin
bool DeleteInputPin(size_t pinIndex)
Deletes the input pin. Invalidates the pin reference given.
Definition Node.cpp:310
NAV::Node::_hasConfig
bool _hasConfig
Flag if the config window should be shown.
Definition Node.hpp:525
NAV::TsDeque::extract_front
auto extract_front()
Returns a copy of the first element in the container and removes it from the container.
Definition TsDeque.hpp:494
NAV::gui::NodeEditorApplication::windowFontRatio
static float windowFontRatio()
Ratio to multiply for GUI window elements.
Definition NodeEditorApplication.cpp:1922
imgui_ex.hpp
ImGui extensions.
ImGui::InputDoubleL
bool InputDoubleL(const char *label, double *v, double v_min, double v_max, double step, double step_fast, const char *format, ImGuiInputTextFlags flags)
Shows a value limited InputText GUI element for 'double'.
Definition imgui_ex.cpp:294
NAV::BsplineKF::rotateErrorCovariances
void rotateErrorCovariances(Eigen::MatrixXd &P, uint8_t &numStates, const double &sigmaScalingFactorAngRate=3.0, const double &sigmaScalingFactorAccel=3.0)
Rotates the B-spline coefficient error covariances in P, once a new B-spline is introduced.
Definition BsplineKF.cpp:62
NAV::BsplineKF::quadraticBsplines
std::array< double, 3 > quadraticBsplines(const double &ti, const double &splineSpacing=1.0)
Set the points/knots of the four B-splines.
Definition QuadraticBsplines.cpp:16
NAV::BsplineKF::processNoiseMatrix_Q
void processNoiseMatrix_Q(Eigen::MatrixXd &Q, const double &dt, const std::vector< Eigen::VectorXd > &processNoiseVariances, const uint8_t &numStates, const bool &imuCharacteristicsIdentical=false)
Calculates the process noise matrix Q.
Definition BsplineKF.cpp:38
NAV::BsplineKF::rotateCoeffStates
void rotateCoeffStates(Eigen::MatrixXd &x)
Rotates the B-spline coefficient states in the state vector x, once a new B-spline is introduced.
Definition BsplineKF.cpp:56
NAV::BsplineKF::initializeKalmanFilterManually
KalmanFilter initializeKalmanFilterManually(const size_t &numInputPins, const std::vector< PinData > &pinData, const PinDataBsplineKF &pinDataBsplineKF, const uint8_t &numStates, const double &dtInit, std::vector< Eigen::VectorXd > &processNoiseVariances, KalmanFilter &kalmanFilter, const bool &imuCharacteristicsIdentical, const bool &imuBiasesIdentical)
Initializes the BsplineKF manually (i.e. using GUI inputs instead of averaging)
Definition BsplineKF.cpp:123
NAV::BsplineKF::designMatrix_H
Eigen::MatrixXd designMatrix_H(const double &ti, const double &splineSpacing, const Eigen::Matrix3d &DCM_accel, const Eigen::Matrix3d &DCM_gyro, const size_t &pinIndex, const uint8_t &numMeasurements, const uint8_t &numStates, const uint8_t &numStatesEst, const uint8_t &numStatesPerPin)
Calculates the design matrix H.
Definition BsplineKF.cpp:91
NAV::IRWKF::stateTransitionMatrix_Phi
void stateTransitionMatrix_Phi(Eigen::MatrixXd &Phi, const double &dt)
Calculates the state-transition-matrix 𝚽
Definition IRWKF.cpp:76
NAV::IRWKF::designMatrix_H
Eigen::MatrixXd designMatrix_H(const Eigen::Matrix3d &DCM_accel, const Eigen::Matrix3d &DCM_gyro, const size_t &pinIndex, const uint8_t &numMeasurements, const uint8_t &numStates, const uint8_t &numStatesEst, const uint8_t &numStatesPerPin)
Calculates the design matrix H.
Definition IRWKF.cpp:82
NAV::IRWKF::processNoiseMatrix_Q
void processNoiseMatrix_Q(Eigen::MatrixXd &Q, const double &dt, const std::vector< Eigen::VectorXd > &processNoiseVariances, const uint8_t &numStates, const bool &imuCharacteristicsIdentical=false)
Calculates the process noise matrix Q.
Definition IRWKF.cpp:49
NAV::IRWKF::initializeKalmanFilterManually
KalmanFilter initializeKalmanFilterManually(const size_t &numInputPins, const std::vector< PinData > &pinData, const PinDataIRWKF &pinDataIRWKF, const uint8_t &numStates, const double &dtInit, std::vector< Eigen::VectorXd > &processNoiseVariances, KalmanFilter &kalmanFilter, const bool &imuCharacteristicsIdentical, const bool &imuBiasesIdentical)
Initializes the IRWKF manually (i.e. using GUI inputs instead of averaging)
Definition IRWKF.cpp:102
NAV::IRWKF::initializeKalmanFilterAuto
KalmanFilter initializeKalmanFilterAuto(const size_t &nInputPins, const std::vector< PinData > &pinData, const PinDataIRWKF &pinDataIRWKF, const std::vector< size_t > &cumulatedPinIds, const std::vector< std::shared_ptr< const NAV::ImuObs > > &cumulatedImuObs, const bool &initJerkAngAcc, const double &dtInit, const uint8_t &numStates, const uint8_t &numMeasurements, std::vector< Eigen::VectorXd > &processNoiseVariances, KalmanFilter &kalmanFilter)
Initializes the IRWKF automatically, i.e. init values are calculated by averaging the data in the fir...
Definition IRWKF.cpp:357
NAV::flow::ApplyChanges
void ApplyChanges()
Signals that there have been changes to the flow.
Definition FlowManager.cpp:682
NAV::gui::widgets::InputDouble3WithUnit
InputWithUnitChange InputDouble3WithUnit(const char *label, float itemWidth, float unitWidth, double v[3], U &combo_current_item, const char *combo_items_separated_by_zeros, const char *format="%.6f", ImGuiInputTextFlags flags=0, int combo_popup_max_height_in_items=-1)
Shows an InputText GUI element to modify the provided value and also set its unit.
Definition InputWithUnit.hpp:325
NAV::gui::widgets::EnumCombo
bool EnumCombo(const char *label, T &enumeration, size_t startIdx=0)
Combo representing an enumeration.
Definition EnumCombo.hpp:30
NAV::gui::widgets::HelpMarker
void HelpMarker(const char *desc, const char *symbol="(?)")
Text Help Marker, e.g. '(?)', with Tooltip.
Definition HelpMarker.cpp:14
NAV::to_json
void to_json(json &j, const Node &node)
Converts the provided node into a json object.
Definition Node.cpp:1060
NAV::from_json
void from_json(const json &j, Node &node)
Converts the provided json object into a node object.
Definition Node.cpp:1077
NAV::deg2rad
constexpr auto deg2rad(const T &deg)
Convert Degree to Radians.
Definition Units.hpp:21
NAV::PinDataBsplineKF
Sensor information specific to the Bspline-KF.
Definition PinData.hpp:179
NAV::PinDataBsplineKF::initCovarianceCoeffsAngRate
Eigen::VectorXd initCovarianceCoeffsAngRate
GUI selection for the initial covariance of the B-spline coefficients of the angular rate.
Definition PinData.hpp:205
NAV::PinDataBsplineKF::initCoeffsAccelUnit
PinData::AccelerationUnit initCoeffsAccelUnit
Gui selection for the unit of the initial coefficients of the acceleration B-splines.
Definition PinData.hpp:185
NAV::PinDataBsplineKF::initCovarianceCoeffsAccelUnit
PinData::AccelerationVarianceUnit initCovarianceCoeffsAccelUnit
Gui selection for the unit of the initial covariance for the coefficients of acceleration.
Definition PinData.hpp:190
NAV::PinDataBsplineKF::initCoeffsAngRate
Eigen::VectorXd initCoeffsAngRate
GUI selection for the initial B-spline coefficients of the angular rate.
Definition PinData.hpp:200
NAV::PinDataBsplineKF::initCovarianceCoeffsAngRateUnit
PinData::AngRateVarianceUnit initCovarianceCoeffsAngRateUnit
Gui selection for the unit of the initial covariance for the coefficients of angular rate.
Definition PinData.hpp:188
NAV::PinDataBsplineKF::varCoeffsAccelNoise
Eigen::VectorXd varCoeffsAccelNoise
GUI selection for the coeffs of the acceleration process noise (diagonal values)
Definition PinData.hpp:212
NAV::PinDataBsplineKF::initCoeffsAccel
Eigen::VectorXd initCoeffsAccel
GUI selection for the initial B-spline coefficients of the acceleration.
Definition PinData.hpp:202
NAV::PinDataBsplineKF::initCovarianceCoeffsAccel
Eigen::VectorXd initCovarianceCoeffsAccel
GUI selection for the initial covariance of the B-spline coefficients of the acceleration.
Definition PinData.hpp:207
NAV::PinDataBsplineKF::varCoeffsAngRateNoise
Eigen::VectorXd varCoeffsAngRateNoise
GUI selection for the coeffs of the angular rate process noise (diagonal values)
Definition PinData.hpp:210
NAV::PinDataBsplineKF::varCoeffsAngRateUnit
PinData::AngRateVarianceUnit varCoeffsAngRateUnit
GUI selection for the B-spline coeffs of the angular rate process noise (diagonal values)
Definition PinData.hpp:193
NAV::PinDataBsplineKF::varCoeffsAccelUnit
PinData::AccelerationVarianceUnit varCoeffsAccelUnit
GUI selection for the B-spline coeffs of the acceleration process noise (diagonal values)
Definition PinData.hpp:195
NAV::PinDataBsplineKF::initCoeffsAngularRateUnit
PinData::AngRateUnit initCoeffsAngularRateUnit
Gui selection for the unit of the initial coefficients of the angular rate B-splines.
Definition PinData.hpp:183
NAV::PinDataIRWKF
Sensor information specific to the IRW-KF.
Definition PinData.hpp:105
NAV::PinDataIRWKF::initCovarianceJerkUnit
JerkVarianceUnit initCovarianceJerkUnit
Gui selection for the unit of the initial covariance for the jerk.
Definition PinData.hpp:148
NAV::PinDataIRWKF::initAccelerationUnit
PinData::AccelerationUnit initAccelerationUnit
Gui selection for the unit of the initial acceleration.
Definition PinData.hpp:141
NAV::PinDataIRWKF::initAcceleration
Eigen::Vector3d initAcceleration
GUI selection for the initial acceleration.
Definition PinData.hpp:162
NAV::PinDataIRWKF::initAngularAccUnit
AngularAccUnit initAngularAccUnit
Gui selection for the unit of the initial angular acceleration.
Definition PinData.hpp:139
NAV::PinDataIRWKF::initCovarianceAngularAcc
Eigen::Vector3d initCovarianceAngularAcc
GUI selection for the initial covariance diagonal values for angular acceleration (standard deviation...
Definition PinData.hpp:167
NAV::PinDataIRWKF::initAngularRate
Eigen::Vector3d initAngularRate
GUI selection for the initial angular rate.
Definition PinData.hpp:158
NAV::PinDataIRWKF::initCovarianceJerk
Eigen::Vector3d initCovarianceJerk
GUI selection for the initial covariance diagonal values for jerk (standard deviation σ or Variance σ...
Definition PinData.hpp:169
NAV::PinDataIRWKF::varJerkNoise
Eigen::Vector3d varJerkNoise
GUI selection for the jerk process noise diagonal values.
Definition PinData.hpp:174
NAV::PinDataIRWKF::varAngularAccNoiseUnit
AngularAccVarianceUnit varAngularAccNoiseUnit
Gui selection for the unit of the angular acceleration process noise.
Definition PinData.hpp:151
NAV::PinDataIRWKF::initJerk
Eigen::Vector3d initJerk
GUI selection for the initial jerk.
Definition PinData.hpp:164
NAV::PinDataIRWKF::varAngularAccNoise
Eigen::Vector3d varAngularAccNoise
GUI selection for the angular acceleration process noise diagonal values.
Definition PinData.hpp:172
NAV::PinDataIRWKF::JerkVarianceUnit::m2_s6
@ m2_s6
Variance [(m^2)/(s^6), (m^2)/(s^6), (m^2)/(s^6)].
Definition PinData.hpp:130
NAV::PinDataIRWKF::initCovarianceAngularAccUnit
AngularAccVarianceUnit initCovarianceAngularAccUnit
Gui selection for the unit of the initial covariance for the angular acceleration.
Definition PinData.hpp:146
NAV::PinDataIRWKF::initAngularRateUnit
PinData::AngRateUnit initAngularRateUnit
Gui selection for the unit of the initial angular rate.
Definition PinData.hpp:137
NAV::PinDataIRWKF::initAngularAcc
Eigen::Vector3d initAngularAcc
GUI selection for the initial angular acceleration.
Definition PinData.hpp:160
NAV::PinDataIRWKF::varJerkNoiseUnit
JerkVarianceUnit varJerkNoiseUnit
Gui selection for the unit of the jerk process noise.
Definition PinData.hpp:153
NAV::PinDataIRWKF::initJerkUnit
JerkUnit initJerkUnit
Gui selection for the Unit of the initial jerk.
Definition PinData.hpp:143
NAV::PinDataIRWKF::AngularAccVarianceUnit::deg2_s4
@ deg2_s4
Variance [(deg^2)/(s^4), (deg^2)/(s^4), (deg^2)/(s^4)].
Definition PinData.hpp:124
NAV::PinDataIRWKF::AngularAccVarianceUnit::rad2_s4
@ rad2_s4
Variance [(rad^2)/(s^4), (rad^2)/(s^4), (rad^2)/(s^4)].
Definition PinData.hpp:122
NAV::PinData
Information about a sensor which is connected to a certain pin (i.e. sensor characteristics defined i...
Definition PinData.hpp:23
NAV::PinData::measurementUncertaintyAccelerationUnit
AccelerationVarianceUnit measurementUncertaintyAccelerationUnit
Gui selection for the unit of the acceleration's measurement uncertainty.
Definition PinData.hpp:76
NAV::PinData::initCovarianceAngularRate
Eigen::Vector3d initCovarianceAngularRate
GUI selection for the initial covariance diagonal values for angular rate (standard deviation σ or Va...
Definition PinData.hpp:86
NAV::PinData::measurementUncertaintyAngularRate
Eigen::Vector3d measurementUncertaintyAngularRate
Gui selection for the angular rate measurement uncertainty diagonal values.
Definition PinData.hpp:98
NAV::PinData::AccelerationVarianceUnit::m2_s4
@ m2_s4
Variance [(m^2)/(s^4), (m^2)/(s^4), (m^2)/(s^4)].
Definition PinData.hpp:50
NAV::PinData::AccelerationVarianceUnit::m_s2
@ m_s2
Standard deviation [m/s², m/s², m/s²].
Definition PinData.hpp:51
NAV::PinData::initCovarianceBiasAccUnit
AccelerationVarianceUnit initCovarianceBiasAccUnit
Gui selection for the Unit of the initial covariance for the acceleration biases.
Definition PinData.hpp:68
NAV::PinData::varBiasAngRateNoise
Eigen::Vector3d varBiasAngRateNoise
GUI selection for the process noise of the angular rate diagonal values (standard deviation σ or Vari...
Definition PinData.hpp:94
NAV::PinData::initCovarianceBiasAcc
Eigen::Vector3d initCovarianceBiasAcc
GUI selection for the initial covariance diagonal values for acceleration biases (standard deviation ...
Definition PinData.hpp:92
NAV::PinData::initCovarianceAcceleration
Eigen::Vector3d initCovarianceAcceleration
GUI selection for the initial covariance diagonal values for acceleration (standard deviation σ or Va...
Definition PinData.hpp:88
NAV::PinData::measurementUncertaintyAcceleration
Eigen::Vector3d measurementUncertaintyAcceleration
Gui selection for the acceleration measurement uncertainty diagonal values.
Definition PinData.hpp:100
NAV::PinData::varBiasAngRateNoiseUnit
AngRateVarianceUnit varBiasAngRateNoiseUnit
Gui selection for the Unit of the process noise of the angular rate.
Definition PinData.hpp:70
NAV::PinData::initAccelerationBiasUnit
AccelerationUnit initAccelerationBiasUnit
GUI selection for the unit of the initial angular acceleration bias.
Definition PinData.hpp:59
NAV::PinData::measurementUncertaintyAngularRateUnit
AngRateVarianceUnit measurementUncertaintyAngularRateUnit
Gui selection for the unit of the angular rate's measurement uncertainty.
Definition PinData.hpp:74
NAV::PinData::AngRateVarianceUnit::rad2_s2
@ rad2_s2
Variance [rad²/s², rad²/s², rad²/s²].
Definition PinData.hpp:41
NAV::PinData::AngRateVarianceUnit::deg2_s2
@ deg2_s2
Variance [deg²/s², deg²/s², deg²/s²].
Definition PinData.hpp:43
NAV::PinData::AngRateVarianceUnit::deg_s
@ deg_s
Standard deviation [deg/s, deg/s, deg/s].
Definition PinData.hpp:44
NAV::PinData::AngRateVarianceUnit::rad_s
@ rad_s
Standard deviation [rad/s, rad/s, rad/s].
Definition PinData.hpp:42
NAV::PinData::initAccelerationBias
Eigen::Vector3d initAccelerationBias
GUI selection for the initial acceleration bias.
Definition PinData.hpp:83
NAV::PinData::initAngularRateBias
Eigen::Vector3d initAngularRateBias
GUI selection for the initial angular rate bias.
Definition PinData.hpp:81
NAV::PinData::initCovarianceAngularRateUnit
AngRateVarianceUnit initCovarianceAngularRateUnit
Gui selection for the Unit of the initial covariance for the angular rate.
Definition PinData.hpp:62
NAV::PinData::initCovarianceBiasAngRate
Eigen::Vector3d initCovarianceBiasAngRate
GUI selection for the initial covariance diagonal values for angular rate biases (standard deviation ...
Definition PinData.hpp:90
NAV::PinData::initCovarianceAccelerationUnit
AccelerationVarianceUnit initCovarianceAccelerationUnit
Gui selection for the Unit of the initial covariance for the acceleration.
Definition PinData.hpp:64
NAV::PinData::varBiasAccelerationNoise
Eigen::Vector3d varBiasAccelerationNoise
GUI selection for the process noise of the acceleration diagonal values (standard deviation σ or Vari...
Definition PinData.hpp:96
NAV::PinData::initCovarianceBiasAngRateUnit
AngRateVarianceUnit initCovarianceBiasAngRateUnit
Gui selection for the Unit of the initial covariance for the angular rate biases.
Definition PinData.hpp:66
NAV::PinData::varBiasAccelerationNoiseUnit
AccelerationVarianceUnit varBiasAccelerationNoiseUnit
Gui selection for the Unit of the process noise of the acceleration.
Definition PinData.hpp:72
NAV::PinData::initAngularRateBiasUnit
AngRateUnit initAngularRateBiasUnit
GUI selection for the unit of the initial angular rate bias.
Definition PinData.hpp:57
NAV::Pin::Type::Flow
@ Flow
NodeData Trigger.
Definition Pin.hpp:52