INSTINCT Code Coverage Report

Directory: src/
File: Nodes/DataProcessor/KalmanFilter/LooselyCoupledKF.cpp
Date: 2025-11-25 23:34:18
Exec Total Coverage
Lines: 700 1241 56.4%
Functions: 38 54 70.4%
Branches: 1196 3858 31.0%
Line Branch Exec Source
1 // This file is part of INSTINCT, the INS Toolkit for Integrated
2 // Navigation Concepts and Training by the Institute of Navigation of
3 // the University of Stuttgart, Germany.
4 //
5 // This Source Code Form is subject to the terms of the Mozilla Public
6 // License, v. 2.0. If a copy of the MPL was not distributed with this
7 // file, You can obtain one at https://mozilla.org/MPL/2.0/.
8
9 #include "LooselyCoupledKF.hpp"
10
11 #include "NodeData/State/InsGnssLCKFSolution.hpp"
12 #include "NodeData/State/PosVel.hpp"
13 #include "NodeData/State/PosVelAtt.hpp"
14 #include "internal/Node/Pin.hpp"
15 #include "util/Eigen.hpp"
16 #include <memory>
17 #include <numbers>
18 #include <cmath>
19
20 #include <imgui.h>
21 #include <imgui_internal.h>
22 #include "internal/gui/widgets/HelpMarker.hpp"
23 #include "internal/gui/widgets/imgui_ex.hpp"
24 #include "internal/gui/widgets/InputWithUnit.hpp"
25 #include "internal/gui/NodeEditorApplication.hpp"
26
27 #include "internal/FlowManager.hpp"
28 #include <fmt/format.h>
29 #include "NodeRegistry.hpp"
30 #include "Navigation/Constants.hpp"
31 #include "Navigation/Ellipsoid/Ellipsoid.hpp"
32 #include "Navigation/INS/Functions.hpp"
33 #include "Navigation/INS/ProcessNoise.hpp"
34 #include "Navigation/INS/EcefFrame/ErrorEquations.hpp"
35 #include "Navigation/INS/LocalNavFrame/ErrorEquations.hpp"
36 #include "Navigation/Math/Math.hpp"
37 #include "Navigation/Gravity/Gravity.hpp"
38 #include "Navigation/Transformations/Units.hpp"
39 #include "util/Logger.hpp"
40 #include "util/Assert.h"
41
42 #include "NodeData/IMU/ImuObsWDelta.hpp"
43
44 /// @brief Scale factor to convert the attitude error
45 constexpr double SCALE_FACTOR_ATTITUDE = 180. / M_PI;
46 /// @brief Scale factor to convert the latitude and longitude error
47 constexpr double SCALE_FACTOR_LAT_LON = NAV::InsConst::pseudometre;
48 /// @brief Scale factor to convert the acceleration error
49 constexpr double SCALE_FACTOR_ACCELERATION = 1e3 / NAV::InsConst::G_NORM;
50 /// @brief Scale factor to convert the angular rate error
51 constexpr double SCALE_FACTOR_ANGULAR_RATE = 1e3;
52
53 131 NAV::LooselyCoupledKF::LooselyCoupledKF()
54
36/72
✓ Branch 1 taken 131 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 131 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 131 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 131 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 131 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 131 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 131 times.
✗ Branch 22 not taken.
✓ Branch 27 taken 131 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 131 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 131 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 131 times.
✗ Branch 37 not taken.
✓ Branch 39 taken 131 times.
✗ Branch 40 not taken.
✓ Branch 42 taken 131 times.
✗ Branch 43 not taken.
✓ Branch 45 taken 131 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 131 times.
✗ Branch 49 not taken.
✓ Branch 51 taken 131 times.
✗ Branch 52 not taken.
✓ Branch 54 taken 131 times.
✗ Branch 55 not taken.
✓ Branch 57 taken 131 times.
✗ Branch 58 not taken.
✓ Branch 60 taken 131 times.
✗ Branch 61 not taken.
✓ Branch 63 taken 131 times.
✗ Branch 64 not taken.
✓ Branch 66 taken 131 times.
✗ Branch 67 not taken.
✓ Branch 69 taken 131 times.
✗ Branch 70 not taken.
✓ Branch 72 taken 131 times.
✗ Branch 73 not taken.
✓ Branch 75 taken 131 times.
✗ Branch 76 not taken.
✓ Branch 78 taken 131 times.
✗ Branch 79 not taken.
✓ Branch 81 taken 131 times.
✗ Branch 82 not taken.
✓ Branch 84 taken 131 times.
✗ Branch 85 not taken.
✓ Branch 87 taken 131 times.
✗ Branch 88 not taken.
✓ Branch 90 taken 131 times.
✗ Branch 91 not taken.
✓ Branch 93 taken 131 times.
✗ Branch 94 not taken.
✓ Branch 96 taken 131 times.
✗ Branch 97 not taken.
✓ Branch 99 taken 131 times.
✗ Branch 100 not taken.
✓ Branch 102 taken 131 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 131 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 131 times.
✗ Branch 109 not taken.
✓ Branch 113 taken 131 times.
✗ Branch 114 not taken.
 131 : Node(typeStatic())
55 {
56 LOG_TRACE("{}: called", name);
57
58 131 _hasConfig = true;
59 131 _guiConfigDefaultWindowSize = { 822, 936 };
60
61
4/8
✓ Branch 2 taken 131 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 131 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 262 times.
✓ Branch 10 taken 131 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 655 CreateInputPin(
62 "ImuObs", Pin::Type::Flow, { NAV::ImuObs::type(), NAV::ImuObsWDelta::type() }, &LooselyCoupledKF::recvImuObservation,
63 ✗ [](const Node* node, const InputPin& inputPin) {
64 49998 const auto* lckf = static_cast<const LooselyCoupledKF*>(node); // NOLINT(cppcoreguidelines-pro-type-static-cast-downcast)
65
2/4
✓ Branch 1 taken 49998 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49998 times.
✗ Branch 5 not taken.
 49998 return !inputPin.queue.empty() && lckf->_inertialIntegrator.hasInitialPosition();
66 },
67 1); // Priority 1 ensures, that the IMU obs (prediction) is evaluated before the PosVel obs (update)
68
69
1/2
✓ Branch 1 taken 131 times.
✗ Branch 2 not taken.
 131 _dynamicInputPins.addPin(this); // PosVel
70
1/2
✓ Branch 1 taken 131 times.
✗ Branch 2 not taken.
 131 updateInputPins();
71
72
4/8
✓ Branch 2 taken 131 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 131 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 131 times.
✓ Branch 10 taken 131 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 524 CreateOutputPin("PosVelAtt", Pin::Type::Flow, { NAV::InsGnssLCKFSolution::type() });
73 393 }
74
75 296 NAV::LooselyCoupledKF::~LooselyCoupledKF()
76 {
77 LOG_TRACE("{}: called", nameId());
78 296 }
79
80 245 std::string NAV::LooselyCoupledKF::typeStatic()
81 {
82
1/2
✓ Branch 1 taken 245 times.
✗ Branch 2 not taken.
 490 return "INS/GNSS LCKF"; // Loosely-coupled Kalman Filter
83 }
84
85 ✗ std::string NAV::LooselyCoupledKF::type() const
86 {
87 ✗ return typeStatic();
88 }
89
90 114 std::string NAV::LooselyCoupledKF::category()
91 {
92
1/2
✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
 228 return "Data Processor";
93 }
94
95 182 void NAV::LooselyCoupledKF::updateInputPins()
96 {
97 182 size_t pinIdx = INPUT_PORT_INDEX_POS_VEL_ATT_INIT;
98
99 364 auto updatePin = [&](bool pinExists, bool enabled,
100 const char* pinName, Pin::Type pinType, const std::vector<std::string>& dataIdentifier = {},
101 auto callback = nullptr, InputPin::FlowFirableCheckFunc firable = nullptr, int priority = 0) {
102
4/4
✓ Branch 0 taken 347 times.
✓ Branch 1 taken 17 times.
✓ Branch 2 taken 17 times.
✓ Branch 3 taken 330 times.
 364 if (!pinExists && enabled)
103 {
104 17 CreateInputPin(pinName, pinType, dataIdentifier, callback,
105 firable, priority, static_cast<int>(pinIdx));
106 17 _dynamicInputPins.setFirstDynamicPinIdx(_dynamicInputPins.getFirstDynamicPinIdx() + 1);
107 }
108
3/4
✓ Branch 0 taken 17 times.
✓ Branch 1 taken 330 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 17 times.
 347 else if (pinExists && !enabled)
109 {
110 ✗ DeleteInputPin(pinIdx);
111 ✗ _dynamicInputPins.setFirstDynamicPinIdx(_dynamicInputPins.getFirstDynamicPinIdx() - 1);
112 }
113
2/2
✓ Branch 0 taken 34 times.
✓ Branch 1 taken 330 times.
 364 if (enabled) { pinIdx++; }
114 546 };
115
116
6/12
✓ Branch 1 taken 182 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 182 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 182 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 182 times.
✓ Branch 12 taken 182 times.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✓ Branch 19 taken 364 times.
✗ Branch 20 not taken.
 910 updatePin(std::ranges::any_of(inputPins, [](const InputPin& pin) { return pin.dataIdentifier.front() == PosVelAtt::type(); }), _initializeStateOverExternalPin,
117 "Init PVA", Pin::Type::Flow, { NAV::PosVelAtt::type() }, &LooselyCoupledKF::recvPosVelAttInit, nullptr, 3);
118
6/12
✓ Branch 1 taken 182 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 182 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 182 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 182 times.
✓ Branch 12 taken 182 times.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✓ Branch 19 taken 398 times.
✗ Branch 20 not taken.
 944 updatePin(std::ranges::any_of(inputPins, [](const InputPin& pin) { return pin.dataIdentifier.front() == BaroHgt::type(); }), _enableBaroHgt,
119 "BaroHgt", Pin::Type::Flow, { NAV::BaroHgt::type() }, &LooselyCoupledKF::recvBaroHeight, nullptr, -1);
120
121
3/6
✓ Branch 1 taken 182 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 182 times.
✓ Branch 6 taken 398 times.
✗ Branch 7 not taken.
 580 if (std::ranges::count_if(inputPins, [](const InputPin& pin) { return pin.dataIdentifier.front() == PosVel::type(); }) == 0)
122 {
123 ✗ _dynamicInputPins.addPin(this); // PosVel
124 }
125 546 }
126
127 131 void NAV::LooselyCoupledKF::pinAddCallback(Node* node)
128 {
129
4/8
✓ Branch 2 taken 131 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 131 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 131 times.
✓ Branch 10 taken 131 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 524 node->CreateInputPin(
130 "PosVel", Pin::Type::Flow, { NAV::PosVel::type() }, &LooselyCoupledKF::recvPosVelObservation,
131 26961 [](const Node* node, const InputPin& inputPin) {
132 26830 const auto* lckf = static_cast<const LooselyCoupledKF*>(node); // NOLINT(cppcoreguidelines-pro-type-static-cast-downcast)
133
3/6
✓ Branch 1 taken 26830 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 26830 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 26830 times.
✗ Branch 7 not taken.
 26830 return !inputPin.queue.empty() && (!lckf->_initializeStateOverExternalPin || lckf->_inertialIntegrator.hasInitialPosition());
134 },
135 2); // Initially this has higher priority than the IMU obs, to initialize the position from it
136 262 }
137
138 ✗ void NAV::LooselyCoupledKF::pinDeleteCallback(Node* node, size_t pinIdx)
139 {
140 ✗ node->DeleteInputPin(pinIdx);
141 ✗ }
142
143 ✗ void NAV::LooselyCoupledKF::guiConfig()
144 {
145 ✗ float configWidth = 400.0F * gui::NodeEditorApplication::windowFontRatio();
146 ✗ float unitWidth = 150.0F * gui::NodeEditorApplication::windowFontRatio();
147
148 ✗ float taylorOrderWidth = 75.0F * gui::NodeEditorApplication::windowFontRatio();
149
150 ✗ if (_dynamicInputPins.ShowGuiWidgets(size_t(id), inputPins, this))
151 {
152 ✗ flow::ApplyChanges();
153 }
154
155 ✗ if (ImGui::CollapsingHeader(fmt::format("Initialization##{}", size_t(id)).c_str(), ImGuiTreeNodeFlags_DefaultOpen))
156 {
157 ✗ if (ImGui::Checkbox(fmt::format("Initialize over pin##{}", size_t(id)).c_str(), &_initializeStateOverExternalPin))
158 {
159 ✗ updateInputPins();
160 ✗ flow::ApplyChanges();
161 }
162 ✗ if (!_initializeStateOverExternalPin)
163 {
164 ✗ ImGui::SetNextItemWidth(80 * gui::NodeEditorApplication::windowFontRatio());
165 ✗ if (ImGui::DragDouble(fmt::format("##initalRollPitchYaw(0) {}", size_t(id)).c_str(),
166 _initalRollPitchYaw.data(), 1.0F, -180.0, 180.0, "%.3f °"))
167 {
168 ✗ flow::ApplyChanges();
169 }
170 ✗ ImGui::SameLine();
171 ✗ ImGui::SetNextItemWidth(80 * gui::NodeEditorApplication::windowFontRatio());
172 ✗ if (ImGui::DragDouble(fmt::format("##initalRollPitchYaw(1) {}", size_t(id)).c_str(),
173 ✗ &_initalRollPitchYaw[1], 1.0F, -90.0, 90.0, "%.3f °"))
174 {
175 ✗ flow::ApplyChanges();
176 }
177 ✗ ImGui::SameLine();
178 ✗ ImGui::SetNextItemWidth(80 * gui::NodeEditorApplication::windowFontRatio());
179 ✗ if (ImGui::DragDouble(fmt::format("##initalRollPitchYaw(2) {}", size_t(id)).c_str(),
180 ✗ &_initalRollPitchYaw[2], 1.0, -180.0, 180.0, "%.3f °"))
181 {
182 ✗ flow::ApplyChanges();
183 }
184 ✗ ImGui::SameLine();
185 ✗ ImGui::TextUnformatted("Roll, Pitch, Yaw");
186 }
187 }
188
189 ✗ if (ImGui::CollapsingHeader(fmt::format("IMU Integrator settings##{}", size_t(id)).c_str(), ImGuiTreeNodeFlags_DefaultOpen))
190 {
191 ✗ if (InertialIntegratorGui(std::to_string(size_t(id)).c_str(), _inertialIntegrator, _preferAccelerationOverDeltaMeasurements))
192 {
193 ✗ flow::ApplyChanges();
194 }
195 }
196 ✗ if (ImGui::CollapsingHeader(fmt::format("Kalman Filter settings##{}", size_t(id)).c_str(), ImGuiTreeNodeFlags_DefaultOpen))
197 {
198 ✗ if (_phiCalculationAlgorithm == PhiCalculationAlgorithm::Taylor)
199 {
200 ✗ ImGui::SetNextItemWidth(configWidth - taylorOrderWidth);
201 }
202 else
203 {
204 ✗ ImGui::SetNextItemWidth(configWidth + ImGui::GetStyle().ItemSpacing.x);
205 }
206 ✗ if (auto phiCalculationAlgorithm = static_cast<int>(_phiCalculationAlgorithm);
207 ✗ ImGui::Combo(fmt::format("##Phi calculation algorithm {}", size_t(id)).c_str(), &phiCalculationAlgorithm, "Van Loan\0Taylor\0\0"))
208 {
209 ✗ _phiCalculationAlgorithm = static_cast<decltype(_phiCalculationAlgorithm)>(phiCalculationAlgorithm);
210 ✗ LOG_DEBUG("{}: Phi calculation algorithm changed to {}", nameId(), fmt::underlying(_phiCalculationAlgorithm));
211 ✗ flow::ApplyChanges();
212 }
213
214 ✗ if (_phiCalculationAlgorithm == PhiCalculationAlgorithm::Taylor)
215 {
216 ✗ ImGui::SameLine();
217 ✗ ImGui::SetNextItemWidth(taylorOrderWidth);
218 ✗ if (ImGui::InputIntL(fmt::format("##Phi calculation Taylor Order {}", size_t(id)).c_str(), &_phiCalculationTaylorOrder, 1, 9))
219 {
220 ✗ LOG_DEBUG("{}: Phi calculation Taylor Order changed to {}", nameId(), _phiCalculationTaylorOrder);
221 ✗ flow::ApplyChanges();
222 }
223 }
224 ✗ ImGui::SameLine();
225 ✗ ImGui::SetCursorPosX(ImGui::GetCursorPosX() - ImGui::GetStyle().ItemSpacing.x + ImGui::GetStyle().ItemInnerSpacing.x);
226 ✗ ImGui::Text("Phi calculation algorithm%s", _phiCalculationAlgorithm == PhiCalculationAlgorithm::Taylor ? " (up to order)" : "");
227
228 ✗ ImGui::SetNextItemWidth(configWidth + ImGui::GetStyle().ItemSpacing.x);
229 ✗ if (auto qCalculationAlgorithm = static_cast<int>(_qCalculationAlgorithm);
230 ✗ ImGui::Combo(fmt::format("Q calculation algorithm##{}", size_t(id)).c_str(), &qCalculationAlgorithm, "Van Loan\0Taylor 1st Order (Groves 2013)\0\0"))
231 {
232 ✗ _qCalculationAlgorithm = static_cast<decltype(_qCalculationAlgorithm)>(qCalculationAlgorithm);
233 ✗ LOG_DEBUG("{}: Q calculation algorithm changed to {}", nameId(), fmt::underlying(_qCalculationAlgorithm));
234 ✗ flow::ApplyChanges();
235 }
236 ✗ if (ImGui::Checkbox(fmt::format("Enable barometric height pin##{}", size_t(id)).c_str(), &_enableBaroHgt))
237 {
238 ✗ updateInputPins();
239 ✗ flow::ApplyChanges();
240 }
241
242 ✗ ImGui::Separator();
243
244 // ###########################################################################################################
245 // Q - System/Process noise covariance matrix
246 // ###########################################################################################################
247
248 ✗ auto inputVector3WithUnit = [&](const char* title, Eigen::Vector3d& data, auto& unit, const char* combo_items_separated_by_zeros, const char* format) {
249 ✗ if (auto response = gui::widgets::InputDouble3WithUnit(fmt::format("{}##{}", title, size_t(id)).c_str(), configWidth, unitWidth,
250 data.data(), unit, combo_items_separated_by_zeros,
251 format, ImGuiInputTextFlags_CharsScientific))
252 {
253 ✗ if (response == gui::widgets::InputWithUnitChange_Input) { LOG_DEBUG("{}: {} changed to {}", nameId(), title, data.transpose()); }
254 ✗ if (response == gui::widgets::InputWithUnitChange_Unit) { LOG_DEBUG("{}: {} unit changed to {}", nameId(), title, fmt::underlying(unit)); }
255 ✗ flow::ApplyChanges();
256 }
257 ✗ };
258
259 ✗ ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
260 ✗ if (ImGui::TreeNode(fmt::format("Q - System/Process noise covariance matrix##{}", size_t(id)).c_str()))
261 {
262 // --------------------------------------------- Accelerometer -----------------------------------------------
263 ✗ if (_qCalculationAlgorithm == QCalculationAlgorithm::VanLoan)
264 {
265 ✗ ImGui::SetNextItemWidth(configWidth + ImGui::GetStyle().ItemSpacing.x);
266 ✗ if (auto randomProcessAccel = static_cast<int>(_randomProcessAccel);
267 ✗ ImGui::Combo(fmt::format("Random Process Accelerometer##{}", size_t(id)).c_str(), &randomProcessAccel, "Random Walk\0"
268 "Gauss-Markov 1st Order\0\0"))
269 {
270 ✗ _randomProcessAccel = static_cast<decltype(_randomProcessAccel)>(randomProcessAccel);
271 ✗ LOG_DEBUG("{}: randomProcessAccel changed to {}", nameId(), fmt::underlying(_randomProcessAccel));
272 ✗ flow::ApplyChanges();
273 }
274 }
275
276 ✗ inputVector3WithUnit("Standard deviation of the noise on the\naccelerometer specific-force measurements",
277 ✗ _stdev_ra, _stdevAccelNoiseUnits, MakeComboItems<Units::ImuAccelerometerFilterNoiseUnits>().c_str(), "%.2e");
278 ✗ inputVector3WithUnit(fmt::format("Standard deviation σ of the accel {}",
279 ✗ _qCalculationAlgorithm == QCalculationAlgorithm::Taylor1
280 ✗ ? "dynamic bias, in σ²/τ"
281 ✗ : (_randomProcessAccel == RandomProcess::RandomWalk ? "bias noise" : "bias noise, in √(2σ²β)"))
282 .c_str(),
283 ✗ _stdev_bad, _stdevAccelBiasUnits, MakeComboItems<Units::ImuAccelerometerFilterBiasUnits>().c_str(), "%.2e");
284
285 ✗ if (_randomProcessAccel == RandomProcess::GaussMarkov1 || _qCalculationAlgorithm == QCalculationAlgorithm::Taylor1)
286 {
287 ✗ int unitCorrelationLength = 0;
288 ✗ if (gui::widgets::InputDouble3LWithUnit(fmt::format("Correlation length τ of the accel dynamic bias##Correlation length τ of the accel dynamic bias {}", size_t(id)).c_str(),
289 configWidth, unitWidth, _tau_bad.data(), 0., std::numeric_limits<double>::max(),
290 unitCorrelationLength, "s\0\0", "%.2e", ImGuiInputTextFlags_CharsScientific))
291 {
292 ✗ LOG_DEBUG("{}: tau_bad changed to {}", nameId(), _tau_bad);
293 ✗ flow::ApplyChanges();
294 }
295 }
296
297 ✗ ImGui::SetCursorPosY(ImGui::GetCursorPosY() + 20.F);
298
299 // ----------------------------------------------- Gyroscope -------------------------------------------------
300
301 ✗ if (_qCalculationAlgorithm == QCalculationAlgorithm::VanLoan)
302 {
303 ✗ ImGui::SetNextItemWidth(configWidth + ImGui::GetStyle().ItemSpacing.x);
304 ✗ if (auto randomProcessGyro = static_cast<int>(_randomProcessGyro);
305 ✗ ImGui::Combo(fmt::format("Random Process Gyroscope##{}", size_t(id)).c_str(), &randomProcessGyro, "Random Walk\0"
306 "Gauss-Markov 1st Order\0\0"))
307 {
308 ✗ _randomProcessGyro = static_cast<decltype(_randomProcessGyro)>(randomProcessGyro);
309 ✗ LOG_DEBUG("{}: randomProcessGyro changed to {}", nameId(), fmt::underlying(_randomProcessGyro));
310 ✗ flow::ApplyChanges();
311 }
312 }
313
314 ✗ inputVector3WithUnit("Standard deviation of the noise on\nthe gyro angular-rate measurements",
315 ✗ _stdev_rg, _stdevGyroNoiseUnits, MakeComboItems<Units::ImuGyroscopeFilterNoiseUnits>().c_str(), "%.2e");
316 ✗ inputVector3WithUnit(fmt::format("Standard deviation σ of the gyro {}",
317 ✗ _qCalculationAlgorithm == QCalculationAlgorithm::Taylor1
318 ✗ ? "dynamic bias, in σ²/τ"
319 ✗ : (_randomProcessGyro == RandomProcess::RandomWalk ? "bias noise" : "bias noise, in √(2σ²β)"))
320 .c_str(),
321 ✗ _stdev_bgd, _stdevGyroBiasUnits, MakeComboItems<Units::ImuGyroscopeFilterBiasUnits>().c_str(), "%.2e");
322
323 ✗ if (_randomProcessGyro == RandomProcess::GaussMarkov1 || _qCalculationAlgorithm == QCalculationAlgorithm::Taylor1)
324 {
325 ✗ int unitCorrelationLength = 0;
326 ✗ if (gui::widgets::InputDouble3LWithUnit(fmt::format("Correlation length τ of the gyro {}##Correlation length τ of the gyro dynamic bias {}",
327 ✗ _qCalculationAlgorithm == QCalculationAlgorithm::VanLoan ? "bias noise" : "dynamic bias", size_t(id))
328 .c_str(),
329 configWidth, unitWidth, _tau_bgd.data(), 0., std::numeric_limits<double>::max(),
330 unitCorrelationLength, "s\0\0", "%.2e", ImGuiInputTextFlags_CharsScientific))
331 {
332 ✗ LOG_DEBUG("{}: tau_bgd changed to {}", nameId(), _tau_bgd);
333 ✗ flow::ApplyChanges();
334 }
335 }
336
337 ✗ ImGui::SetCursorPosY(ImGui::GetCursorPosY() + 20.F);
338
339 // ----------------------------------------- Barometer -------------------------------------------
340 ✗ if (_enableBaroHgt)
341 {
342 ✗ if (gui::widgets::InputDoubleWithUnit(fmt::format("Standard deviation of the baro height bias##{}", size_t(id))
343 .c_str(),
344 ✗ configWidth, unitWidth, &_stdevBaroHeightBias, _stdevBaroHeightBiasUnits,
345 "m\0\0", 0.0, 0.0, "%.2e", ImGuiInputTextFlags_CharsScientific))
346 {
347 ✗ LOG_DEBUG("{}: stdevBaroHeightBias changed to: {}", nameId(), _stdevBaroHeightBias);
348 ✗ LOG_DEBUG("{}: stdevBaroHeightBiasUnits changed to: {}", nameId(), fmt::underlying(_stdevBaroHeightBiasUnits));
349 ✗ flow::ApplyChanges();
350 }
351 ✗ ImGui::SameLine();
352 ✗ gui::widgets::HelpMarker("Random walk");
353 ✗ if (gui::widgets::InputDoubleWithUnit(fmt::format("Standard deviation of the baro scale##{}", size_t(id))
354 .c_str(),
355 ✗ configWidth, unitWidth, &_stdevBaroHeightScale, _stdevBaroHeightScaleUnits,
356 "m/m\0\0", 0.0, 0.0, "%.2e", ImGuiInputTextFlags_CharsScientific))
357 {
358 ✗ LOG_DEBUG("{}: stdevBaroHeightScale changed to: {}", nameId(), _stdevBaroHeightScale);
359 ✗ LOG_DEBUG("{}: stdevBaroHeightScaleUnits changed to: {}", nameId(), fmt::underlying(_stdevBaroHeightScaleUnits));
360 ✗ flow::ApplyChanges();
361 }
362 ✗ ImGui::SameLine();
363 ✗ gui::widgets::HelpMarker("Random walk");
364 }
365
366 ✗ ImGui::TreePop();
367 }
368
369 // ###########################################################################################################
370 // Measurement Uncertainties 𝐑
371 // ###########################################################################################################
372
373 ✗ ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
374 ✗ if (ImGui::TreeNode(fmt::format("R - Measurement noise covariance matrix##{}", size_t(id)).c_str()))
375 {
376 ✗ float curPosX = ImGui::GetCursorPosX();
377 ✗ if (ImGui::Checkbox(fmt::format("##Override R Position {}", size_t(id)).c_str(), &_gnssMeasurementUncertaintyPositionOverride))
378 {
379 ✗ flow::ApplyChanges();
380 }
381 ✗ if (ImGui::IsItemHovered()) { ImGui::SetTooltip("Override the position variance in the measurements with these values."); }
382 ✗ ImGui::SameLine();
383 ✗ float checkWidth = ImGui::GetCursorPosX() - curPosX;
384 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("{} {} of the GNSS position measurements##{}",
385 ✗ NAV::to_string(_inertialIntegrator.getIntegrationFrame()),
386 ✗ _gnssMeasurementUncertaintyPositionUnit == GnssMeasurementUncertaintyPositionUnit::meter2
387 ✗ ? "Variance"
388 : "Standard deviation",
389 ✗ size_t(id))
390 .c_str(),
391 ✗ configWidth - checkWidth, unitWidth, _gnssMeasurementUncertaintyPosition.data(), _gnssMeasurementUncertaintyPositionUnit, "m^2, m^2, m^2\0"
392 "m, m, m\0\0",
393 "%.2e", ImGuiInputTextFlags_CharsScientific))
394 {
395 ✗ LOG_DEBUG("{}: gnssMeasurementUncertaintyPosition changed to {}", nameId(), _gnssMeasurementUncertaintyPosition.transpose());
396 ✗ LOG_DEBUG("{}: gnssMeasurementUncertaintyPositionUnit changed to {}", nameId(), fmt::underlying(_gnssMeasurementUncertaintyPositionUnit));
397 ✗ flow::ApplyChanges();
398 }
399
400 ✗ if (ImGui::Checkbox(fmt::format("##Override R Velocity {}", size_t(id)).c_str(), &_gnssMeasurementUncertaintyVelocityOverride))
401 {
402 ✗ flow::ApplyChanges();
403 }
404 ✗ if (ImGui::IsItemHovered()) { ImGui::SetTooltip("Override the velocity variance in the measurements with these values."); }
405 ✗ ImGui::SameLine();
406 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("{} {} of the GNSS velocity measurements##{}",
407 ✗ NAV::to_string(_inertialIntegrator.getIntegrationFrame()),
408 ✗ _gnssMeasurementUncertaintyVelocityUnit == GnssMeasurementUncertaintyVelocityUnit::m2_s2 ? "Variance" : "Standard deviation",
409 ✗ size_t(id))
410 .c_str(),
411 ✗ configWidth - checkWidth, unitWidth, _gnssMeasurementUncertaintyVelocity.data(), _gnssMeasurementUncertaintyVelocityUnit, "m^2/s^2\0"
412 "m/s\0\0",
413 "%.2e", ImGuiInputTextFlags_CharsScientific))
414 {
415 ✗ LOG_DEBUG("{}: gnssMeasurementUncertaintyVelocity changed to {}", nameId(), _gnssMeasurementUncertaintyVelocity);
416 ✗ LOG_DEBUG("{}: gnssMeasurementUncertaintyVelocityUnit changed to {}", nameId(), fmt::underlying(_gnssMeasurementUncertaintyVelocityUnit));
417 ✗ flow::ApplyChanges();
418 }
419
420 ✗ if (_enableBaroHgt)
421 {
422 ✗ if (ImGui::Checkbox(fmt::format("##Override R Baro Height {}", size_t(id)).c_str(), &_baroHeightMeasurementUncertaintyOverride))
423 {
424 ✗ flow::ApplyChanges();
425 }
426 ✗ if (ImGui::IsItemHovered()) { ImGui::SetTooltip("Override the barometric height variance in the measurements with this value."); }
427 ✗ ImGui::SameLine();
428 ✗ if (gui::widgets::InputDoubleWithUnit(fmt::format("{} of the barometric height measurements##{}", _barometricHeightMeasurementUncertaintyUnit == BaroHeightMeasurementUncertaintyUnit::m2 ? "Variance" : "Standard deviation",
429 ✗ size_t(id))
430 .c_str(),
431 ✗ configWidth - checkWidth, unitWidth, &_barometricHeightMeasurementUncertainty, _barometricHeightMeasurementUncertaintyUnit, "m^2\0"
432 "m\0\0",
433
434 0.0, 0.0, "%.2e", ImGuiInputTextFlags_CharsScientific))
435 {
436 ✗ LOG_DEBUG("{}: barometricHeightMeasurementUncertainty changed to {}", nameId(), _barometricHeightMeasurementUncertainty);
437 ✗ LOG_DEBUG("{}: barometricHeightMeasurementUncertaintyUnit changed to {}", nameId(), fmt::underlying(_barometricHeightMeasurementUncertaintyUnit));
438 ✗ flow::ApplyChanges();
439 }
440 }
441
442 ✗ ImGui::TreePop();
443 }
444
445 // ###########################################################################################################
446 // 𝐏 Error covariance matrix
447 // ###########################################################################################################
448
449 ✗ ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
450 ✗ if (ImGui::TreeNode(fmt::format("P Error covariance matrix (init)##{}", size_t(id)).c_str()))
451 {
452 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Position covariance ({})##{}",
453 ✗ _initCovariancePositionUnit == InitCovariancePositionUnit::rad2_rad2_m2
454 ✗ || _initCovariancePositionUnit == InitCovariancePositionUnit::meter2
455 ✗ ? "Variance σ²"
456 : "Standard deviation σ",
457 ✗ size_t(id))
458 .c_str(),
459 ✗ configWidth, unitWidth, _initCovariancePosition.data(), _initCovariancePositionUnit, "rad^2, rad^2, m^2\0"
460 "rad, rad, m\0"
461 "m^2, m^2, m^2\0"
462 "m, m, m\0\0",
463 "%.2e", ImGuiInputTextFlags_CharsScientific))
464 {
465 ✗ LOG_DEBUG("{}: initCovariancePosition changed to {}", nameId(), _initCovariancePosition);
466 ✗ LOG_DEBUG("{}: initCovariancePositionUnit changed to {}", nameId(), fmt::underlying(_initCovariancePositionUnit));
467 ✗ flow::ApplyChanges();
468 }
469
470 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Velocity covariance ({})##{}",
471 ✗ _initCovarianceVelocityUnit == InitCovarianceVelocityUnit::m2_s2
472 ✗ ? "Variance σ²"
473 : "Standard deviation σ",
474 ✗ size_t(id))
475 .c_str(),
476 ✗ configWidth, unitWidth, _initCovarianceVelocity.data(), _initCovarianceVelocityUnit, "m^2/s^2\0"
477 "m/s\0\0",
478 "%.2e", ImGuiInputTextFlags_CharsScientific))
479 {
480 ✗ LOG_DEBUG("{}: initCovarianceVelocity changed to {}", nameId(), _initCovarianceVelocity);
481 ✗ LOG_DEBUG("{}: initCovarianceVelocityUnit changed to {}", nameId(), fmt::underlying(_initCovarianceVelocityUnit));
482 ✗ flow::ApplyChanges();
483 }
484
485 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Flight Angles covariance ({})##{}",
486 ✗ _initCovarianceAttitudeAnglesUnit == InitCovarianceAttitudeAnglesUnit::rad2
487 ✗ || _initCovarianceAttitudeAnglesUnit == InitCovarianceAttitudeAnglesUnit::deg2
488 ✗ ? "Variance σ²"
489 : "Standard deviation σ",
490 ✗ size_t(id))
491 .c_str(),
492 ✗ configWidth, unitWidth, _initCovarianceAttitudeAngles.data(), _initCovarianceAttitudeAnglesUnit, "rad^2\0"
493 "deg^2\0"
494 "rad\0"
495 "deg\0\0",
496 "%.2e", ImGuiInputTextFlags_CharsScientific))
497 {
498 ✗ LOG_DEBUG("{}: initCovarianceAttitudeAngles changed to {}", nameId(), _initCovarianceAttitudeAngles);
499 ✗ LOG_DEBUG("{}: initCovarianceAttitudeAnglesUnit changed to {}", nameId(), fmt::underlying(_initCovarianceAttitudeAnglesUnit));
500 ✗ flow::ApplyChanges();
501 }
502 ✗ ImGui::SameLine();
503 ✗ gui::widgets::HelpMarker(_inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::ECEF
504 ? "Angles rotating the ECEF frame into the body frame."
505 : "Angles rotating the local navigation frame into the body frame (Roll, Pitch, Yaw)");
506
507 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Accelerometer Bias covariance ({})##{}",
508 ✗ _initCovarianceBiasAccelUnit == InitCovarianceBiasAccelUnit::m2_s4
509 ✗ ? "Variance σ²"
510 : "Standard deviation σ",
511 ✗ size_t(id))
512 .c_str(),
513 ✗ configWidth, unitWidth, _initCovarianceBiasAccel.data(), _initCovarianceBiasAccelUnit, "m^2/s^4\0"
514 "m/s^2\0\0",
515 "%.2e", ImGuiInputTextFlags_CharsScientific))
516 {
517 ✗ LOG_DEBUG("{}: initCovarianceBiasAccel changed to {}", nameId(), _initCovarianceBiasAccel);
518 ✗ LOG_DEBUG("{}: initCovarianceBiasAccelUnit changed to {}", nameId(), fmt::underlying(_initCovarianceBiasAccelUnit));
519 ✗ flow::ApplyChanges();
520 }
521
522 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Gyroscope Bias covariance ({})##{}",
523 ✗ _initCovarianceBiasGyroUnit == InitCovarianceBiasGyroUnit::rad2_s2
524 ✗ || _initCovarianceBiasGyroUnit == InitCovarianceBiasGyroUnit::deg2_s2
525 ✗ ? "Variance σ²"
526 : "Standard deviation σ",
527 ✗ size_t(id))
528 .c_str(),
529 ✗ configWidth, unitWidth, _initCovarianceBiasGyro.data(), _initCovarianceBiasGyroUnit, "rad^2/s^2\0"
530 "deg^2/s^2\0"
531 "rad/s\0"
532 "deg/s\0\0",
533 "%.2e", ImGuiInputTextFlags_CharsScientific))
534 {
535 ✗ LOG_DEBUG("{}: initCovarianceBiasGyro changed to {}", nameId(), _initCovarianceBiasGyro);
536 ✗ LOG_DEBUG("{}: initCovarianceBiasGyroUnit changed to {}", nameId(), fmt::underlying(_initCovarianceBiasGyroUnit));
537 ✗ flow::ApplyChanges();
538 }
539 ✗ if (_enableBaroHgt)
540 {
541 ✗ if (gui::widgets::InputDoubleWithUnit(fmt::format("Height Bias covariance ({})##{}",
542 ✗ _initCovarianceBiasHeightUnit == InitCovarianceBiasHeightUnit::m2
543 ✗ ? "Variance σ²"
544 : "Standard deviation σ",
545 ✗ size_t(id))
546 .c_str(),
547 ✗ configWidth, unitWidth, &_initCovarianceBiasHeight, _initCovarianceBiasHeightUnit, "m^2\0"
548 "m\0\0",
549 0.0, 0.0, "%.2e", ImGuiInputTextFlags_CharsScientific))
550 {
551 ✗ LOG_DEBUG("{}: initCovarianceBiasHeight changed to {}", nameId(), _initCovarianceBiasHeight);
552 ✗ LOG_DEBUG("{}: initCovarianceBiasHeightUnit changed to {}", nameId(), fmt::underlying(_initCovarianceBiasHeightUnit));
553 ✗ flow::ApplyChanges();
554 }
555 ✗ if (gui::widgets::InputDoubleWithUnit(fmt::format("Height Scale covariance ({})##{}",
556 ✗ _initCovarianceScaleHeightUnit == InitCovarianceScaleHeight::m2_m2
557 ✗ ? "Variance σ²"
558 : "Standard deviation σ",
559 ✗ size_t(id))
560 .c_str(),
561 ✗ configWidth, unitWidth, &_initCovarianceScaleHeight, _initCovarianceScaleHeightUnit, "m^2/m^2\0"
562 "m/m\0\0",
563 0.0, 0.0, "%.2e", ImGuiInputTextFlags_CharsScientific))
564 {
565 ✗ LOG_DEBUG("{}: initCovarianceScaleHeight changed to {}", nameId(), _initCovarianceScaleHeight);
566 ✗ LOG_DEBUG("{}: initCovarianceScaleHeightUnit changed to {}", nameId(), fmt::underlying(_initCovarianceScaleHeightUnit));
567 ✗ flow::ApplyChanges();
568 }
569 }
570
571 ✗ ImGui::TreePop();
572 }
573
574 // ###########################################################################################################
575 // IMU biases
576 // ###########################################################################################################
577
578 ✗ ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
579 ✗ if (ImGui::TreeNode(fmt::format("IMU biases (init)##{}", size_t(id)).c_str()))
580 {
581 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Accelerometer biases##{}", size_t(id)).c_str(),
582 ✗ configWidth, unitWidth, _initBiasAccel.data(), _initBiasAccelUnit, "µg\0m/s^2\0\0",
583 "%.2e", ImGuiInputTextFlags_CharsScientific))
584 {
585 ✗ LOG_DEBUG("{}: initBiasAccel changed to {}", nameId(), _initBiasAccel.transpose());
586 ✗ LOG_DEBUG("{}: initBiasAccelUnit changed to {}", nameId(), fmt::underlying(_initBiasAccelUnit));
587 ✗ flow::ApplyChanges();
588 }
589 ✗ ImGui::SameLine();
590 ✗ gui::widgets::HelpMarker("In body frame coordinates");
591 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Gyro biases##{}", size_t(id)).c_str(),
592 ✗ configWidth, unitWidth, _initBiasGyro.data(), _initBiasGyroUnit, "rad/s\0deg/s\0\0",
593 "%.2e", ImGuiInputTextFlags_CharsScientific))
594 {
595 ✗ LOG_DEBUG("{}: initBiasGyro changed to {}", nameId(), _initBiasGyro.transpose());
596 ✗ LOG_DEBUG("{}: initBiasGyroUnit changed to {}", nameId(), fmt::underlying(_initBiasGyroUnit));
597 ✗ flow::ApplyChanges();
598 }
599 ✗ ImGui::SameLine();
600 ✗ gui::widgets::HelpMarker("In body frame coordinates");
601
602 ✗ ImGui::TreePop();
603 }
604
605 ✗ ImGui::SetNextItemOpen(false, ImGuiCond_FirstUseEver);
606 ✗ if (ImGui::TreeNode(fmt::format("Kalman Filter matrices##{}", size_t(id)).c_str()))
607 {
608 ✗ _kalmanFilter.showKalmanFilterMatrixViews(std::to_string(size_t(id)).c_str());
609 ✗ ImGui::TreePop();
610 }
611
612 ✗ ImGui::Separator();
613
614 ✗ if (ImGui::Checkbox(fmt::format("Rank check for Kalman filter matrices##{}", size_t(id)).c_str(), &_checkKalmanMatricesRanks))
615 {
616 ✗ LOG_DEBUG("{}: checkKalmanMatricesRanks {}", nameId(), _checkKalmanMatricesRanks);
617 ✗ flow::ApplyChanges();
618 }
619 }
620 ✗ }
621
622 ✗ [[nodiscard]] json NAV::LooselyCoupledKF::save() const
623 {
624 LOG_TRACE("{}: called", nameId());
625
626 ✗ json j;
627
628 ✗ j["dynamicInputPins"] = _dynamicInputPins;
629 ✗ j["inertialIntegrator"] = _inertialIntegrator;
630 ✗ j["preferAccelerationOverDeltaMeasurements"] = _preferAccelerationOverDeltaMeasurements;
631 ✗ j["initalRollPitchYaw"] = _initalRollPitchYaw;
632 ✗ j["initializeStateOverExternalPin"] = _initializeStateOverExternalPin;
633
634 ✗ j["checkKalmanMatricesRanks"] = _checkKalmanMatricesRanks;
635
636 ✗ j["phiCalculationAlgorithm"] = _phiCalculationAlgorithm;
637 ✗ j["phiCalculationTaylorOrder"] = _phiCalculationTaylorOrder;
638 ✗ j["qCalculationAlgorithm"] = _qCalculationAlgorithm;
639
640 ✗ j["enableBaroHgt"] = _enableBaroHgt;
641
642 ✗ j["randomProcessAccel"] = _randomProcessAccel;
643 ✗ j["randomProcessGyro"] = _randomProcessGyro;
644 ✗ j["stdev_ra"] = _stdev_ra;
645 ✗ j["stdevAccelNoiseUnits"] = _stdevAccelNoiseUnits;
646 ✗ j["stdev_rg"] = _stdev_rg;
647 ✗ j["stdevGyroNoiseUnits"] = _stdevGyroNoiseUnits;
648 ✗ j["stdev_bad"] = _stdev_bad;
649 ✗ j["tau_bad"] = _tau_bad;
650 ✗ j["stdevAccelBiasUnits"] = _stdevAccelBiasUnits;
651 ✗ j["stdev_bgd"] = _stdev_bgd;
652 ✗ j["tau_bgd"] = _tau_bgd;
653 ✗ j["stdevGyroBiasUnits"] = _stdevGyroBiasUnits;
654 ✗ j["stdevBaroHeightBiasUnits"] = _stdevBaroHeightBiasUnits;
655 ✗ j["stdevBaroHeightBias"] = _stdevBaroHeightBias;
656 ✗ j["stdevBaroHeightScaleUnits"] = _stdevBaroHeightScaleUnits;
657 ✗ j["stdevBaroHeightScale"] = _stdevBaroHeightScale;
658
659 ✗ j["gnssMeasurementUncertaintyPositionUnit"] = _gnssMeasurementUncertaintyPositionUnit;
660 ✗ j["gnssMeasurementUncertaintyPosition"] = _gnssMeasurementUncertaintyPosition;
661 ✗ j["gnssMeasurementUncertaintyPositionOverride"] = _gnssMeasurementUncertaintyPositionOverride;
662 ✗ j["gnssMeasurementUncertaintyVelocityUnit"] = _gnssMeasurementUncertaintyVelocityUnit;
663 ✗ j["gnssMeasurementUncertaintyVelocity"] = _gnssMeasurementUncertaintyVelocity;
664 ✗ j["gnssMeasurementUncertaintyVelocityOverride"] = _gnssMeasurementUncertaintyVelocityOverride;
665 ✗ j["barometricHeightMeasurementUncertaintyUnit"] = _barometricHeightMeasurementUncertaintyUnit;
666 ✗ j["barometricHeightMeasurementUncertainty"] = _barometricHeightMeasurementUncertainty;
667 ✗ j["baroHeightMeasurementUncertaintyOverride"] = _baroHeightMeasurementUncertaintyOverride;
668
669 ✗ j["initCovariancePositionUnit"] = _initCovariancePositionUnit;
670 ✗ j["initCovariancePosition"] = _initCovariancePosition;
671 ✗ j["initCovarianceVelocityUnit"] = _initCovarianceVelocityUnit;
672 ✗ j["initCovarianceVelocity"] = _initCovarianceVelocity;
673 ✗ j["initCovarianceAttitudeAnglesUnit"] = _initCovarianceAttitudeAnglesUnit;
674 ✗ j["initCovarianceAttitudeAngles"] = _initCovarianceAttitudeAngles;
675 ✗ j["initCovarianceBiasAccelUnit"] = _initCovarianceBiasAccelUnit;
676 ✗ j["initCovarianceBiasAccel"] = _initCovarianceBiasAccel;
677 ✗ j["initCovarianceBiasGyroUnit"] = _initCovarianceBiasGyroUnit;
678 ✗ j["initCovarianceBiasGyro"] = _initCovarianceBiasGyro;
679 ✗ j["initCovarianceBiasHeightUnit"] = _initCovarianceBiasHeightUnit;
680 ✗ j["initCovarianceBiasHeight"] = _initCovarianceBiasHeight;
681 ✗ j["initCovarianceScaleHeightUnit"] = _initCovarianceScaleHeightUnit;
682 ✗ j["initCovarianceScaleHeight"] = _initCovarianceScaleHeight;
683
684 ✗ j["initBiasAccel"] = _initBiasAccel;
685 ✗ j["initBiasAccelUnit"] = _initBiasAccelUnit;
686 ✗ j["initBiasGyro"] = _initBiasGyro;
687 ✗ j["initBiasGyroUnit"] = _initBiasGyroUnit;
688
689 ✗ return j;
690 ✗ }
691
692 17 void NAV::LooselyCoupledKF::restore(json const& j)
693 {
694 LOG_TRACE("{}: called", nameId());
695
696
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("dynamicInputPins"))
697 {
698 17 gui::widgets::from_json(j.at("dynamicInputPins"), _dynamicInputPins, this);
699 17 updateInputPins();
700 }
701
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("inertialIntegrator"))
702 {
703 17 j.at("inertialIntegrator").get_to(_inertialIntegrator);
704 }
705
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("preferAccelerationOverDeltaMeasurements"))
706 {
707 17 j.at("preferAccelerationOverDeltaMeasurements").get_to(_preferAccelerationOverDeltaMeasurements);
708 }
709
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initalRollPitchYaw"))
710 {
711 17 j.at("initalRollPitchYaw").get_to(_initalRollPitchYaw);
712 }
713
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initializeStateOverExternalPin"))
714 {
715 17 j.at("initializeStateOverExternalPin").get_to(_initializeStateOverExternalPin);
716 17 updateInputPins();
717 }
718
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("checkKalmanMatricesRanks"))
719 {
720 17 j.at("checkKalmanMatricesRanks").get_to(_checkKalmanMatricesRanks);
721 }
722
723
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("phiCalculationAlgorithm"))
724 {
725 17 j.at("phiCalculationAlgorithm").get_to(_phiCalculationAlgorithm);
726 }
727
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("phiCalculationTaylorOrder"))
728 {
729 17 j.at("phiCalculationTaylorOrder").get_to(_phiCalculationTaylorOrder);
730 }
731
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("qCalculationAlgorithm"))
732 {
733 17 j.at("qCalculationAlgorithm").get_to(_qCalculationAlgorithm);
734 }
735
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("enableBaroHgt"))
736 {
737 17 j.at("enableBaroHgt").get_to(_enableBaroHgt);
738 17 updateInputPins();
739 }
740 // ------------------------------- 𝐐 System/Process noise covariance matrix ---------------------------------
741
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("randomProcessAccel"))
742 {
743 17 j.at("randomProcessAccel").get_to(_randomProcessAccel);
744 }
745
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("randomProcessGyro"))
746 {
747 17 j.at("randomProcessGyro").get_to(_randomProcessGyro);
748 }
749
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("stdev_ra"))
750 {
751
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 17 _stdev_ra = j.at("stdev_ra");
752 }
753
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("stdevAccelNoiseUnits"))
754 {
755 17 j.at("stdevAccelNoiseUnits").get_to(_stdevAccelNoiseUnits);
756 }
757
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("stdev_rg"))
758 {
759
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 17 _stdev_rg = j.at("stdev_rg");
760 }
761
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("stdevGyroNoiseUnits"))
762 {
763 17 j.at("stdevGyroNoiseUnits").get_to(_stdevGyroNoiseUnits);
764 }
765
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("stdev_bad"))
766 {
767
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 17 _stdev_bad = j.at("stdev_bad");
768 }
769
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("tau_bad"))
770 {
771
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 17 _tau_bad = j.at("tau_bad");
772 }
773
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("stdevAccelBiasUnits"))
774 {
775 17 j.at("stdevAccelBiasUnits").get_to(_stdevAccelBiasUnits);
776 }
777
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("stdev_bgd"))
778 {
779
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 17 _stdev_bgd = j.at("stdev_bgd");
780 }
781
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("tau_bgd"))
782 {
783
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 17 _tau_bgd = j.at("tau_bgd");
784 }
785
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("stdevGyroBiasUnits"))
786 {
787 17 j.at("stdevGyroBiasUnits").get_to(_stdevGyroBiasUnits);
788 }
789
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("stdevBaroHeightBiasUnits"))
790 {
791 17 j.at("stdevBaroHeightBiasUnits").get_to(_stdevBaroHeightBiasUnits);
792 }
793
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("stdevBaroHeightBias"))
794 {
795 17 _stdevBaroHeightBias = j.at("stdevBaroHeightBias");
796 }
797
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("stdevBaroHeightScaleUnits"))
798 {
799 17 j.at("stdevBaroHeightScaleUnits").get_to(_stdevBaroHeightScaleUnits);
800 }
801
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("stdevBaroHeightScale"))
802 {
803 17 _stdevBaroHeightScale = j.at("stdevBaroHeightScale");
804 }
805
806 // -------------------------------- 𝐑 Measurement noise covariance matrix -----------------------------------
807
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("gnssMeasurementUncertaintyPositionUnit"))
808 {
809 17 j.at("gnssMeasurementUncertaintyPositionUnit").get_to(_gnssMeasurementUncertaintyPositionUnit);
810 }
811
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("gnssMeasurementUncertaintyPosition"))
812 {
813
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 17 _gnssMeasurementUncertaintyPosition = j.at("gnssMeasurementUncertaintyPosition");
814 }
815
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("gnssMeasurementUncertaintyPositionOverride"))
816 {
817 17 j.at("gnssMeasurementUncertaintyPositionOverride").get_to(_gnssMeasurementUncertaintyPositionOverride);
818 }
819
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("gnssMeasurementUncertaintyVelocityUnit"))
820 {
821 17 j.at("gnssMeasurementUncertaintyVelocityUnit").get_to(_gnssMeasurementUncertaintyVelocityUnit);
822 }
823
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("gnssMeasurementUncertaintyVelocity"))
824 {
825
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 17 _gnssMeasurementUncertaintyVelocity = j.at("gnssMeasurementUncertaintyVelocity");
826 }
827
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("gnssMeasurementUncertaintyVelocityOverride"))
828 {
829 17 j.at("gnssMeasurementUncertaintyVelocityOverride").get_to(_gnssMeasurementUncertaintyVelocityOverride);
830 }
831
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("barometricHeightMeasurementUncertaintyUnit"))
832 {
833 17 j.at("barometricHeightMeasurementUncertaintyUnit").get_to(_barometricHeightMeasurementUncertaintyUnit);
834 }
835
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("barometricHeightMeasurementUncertainty"))
836 {
837 17 _barometricHeightMeasurementUncertainty = j.at("barometricHeightMeasurementUncertainty");
838 }
839
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("baroHeightMeasurementUncertaintyOverride"))
840 {
841 17 j.at("baroHeightMeasurementUncertaintyOverride").get_to(_baroHeightMeasurementUncertaintyOverride);
842 }
843 // -------------------------------------- 𝐏 Error covariance matrix -----------------------------------------
844
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initCovariancePositionUnit"))
845 {
846 17 j.at("initCovariancePositionUnit").get_to(_initCovariancePositionUnit);
847 }
848
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initCovariancePosition"))
849 {
850
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 17 _initCovariancePosition = j.at("initCovariancePosition");
851 }
852
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initCovarianceVelocityUnit"))
853 {
854 17 j.at("initCovarianceVelocityUnit").get_to(_initCovarianceVelocityUnit);
855 }
856
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initCovarianceVelocity"))
857 {
858
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 17 _initCovarianceVelocity = j.at("initCovarianceVelocity");
859 }
860
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initCovarianceAttitudeAnglesUnit"))
861 {
862 17 j.at("initCovarianceAttitudeAnglesUnit").get_to(_initCovarianceAttitudeAnglesUnit);
863 }
864
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initCovarianceAttitudeAngles"))
865 {
866
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 17 _initCovarianceAttitudeAngles = j.at("initCovarianceAttitudeAngles");
867 }
868
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initCovarianceBiasAccelUnit"))
869 {
870 17 j.at("initCovarianceBiasAccelUnit").get_to(_initCovarianceBiasAccelUnit);
871 }
872
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initCovarianceBiasAccel"))
873 {
874
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 17 _initCovarianceBiasAccel = j.at("initCovarianceBiasAccel");
875 }
876
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initCovarianceBiasGyroUnit"))
877 {
878 17 j.at("initCovarianceBiasGyroUnit").get_to(_initCovarianceBiasGyroUnit);
879 }
880
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initCovarianceBiasGyro"))
881 {
882
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 17 _initCovarianceBiasGyro = j.at("initCovarianceBiasGyro");
883 }
884
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initCovarianceBiasHeightUnit"))
885 {
886 17 j.at("initCovarianceBiasHeightUnit").get_to(_initCovarianceBiasHeightUnit);
887 }
888
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initCovarianceBiasHeight"))
889 {
890 17 _initCovarianceBiasHeight = j.at("initCovarianceBiasHeight");
891 }
892
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initCovarianceScaleHeightUnit"))
893 {
894 17 j.at("initCovarianceScaleHeightUnit").get_to(_initCovarianceScaleHeightUnit);
895 }
896
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initCovarianceScaleHeight"))
897 {
898 17 _initCovarianceScaleHeight = j.at("initCovarianceScaleHeight");
899 }
900
901 // ---------------------------------------- Initial IMU biases -------------------------------------------
902
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initBiasAccel"))
903 {
904
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 17 _initBiasAccel = j.at("initBiasAccel");
905 }
906
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initBiasAccelUnit"))
907 {
908 17 _initBiasAccelUnit = j.at("initBiasAccelUnit");
909 }
910
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initBiasGyro"))
911 {
912
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 17 _initBiasGyro = j.at("initBiasGyro");
913 }
914
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initBiasGyroUnit"))
915 {
916 17 _initBiasGyroUnit = j.at("initBiasGyroUnit");
917 }
918 17 }
919
920 51 bool NAV::LooselyCoupledKF::initialize()
921 {
922 LOG_TRACE("{}: called", nameId());
923
924
3/4
✓ Branch 3 taken 102 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 51 times.
✓ Branch 6 taken 51 times.
 102 for (size_t i = _dynamicInputPins.getFirstDynamicPinIdx(); i < _dynamicInputPins.getFirstDynamicPinIdx() + _dynamicInputPins.getNumberOfDynamicPins(); i++)
925 {
926
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 inputPins.at(i).priority = 2; // PosVel used for initialization
927 }
928
929
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 _inertialIntegrator.reset();
930 51 _lastImuObs = nullptr;
931 51 _externalInitTime.reset();
932 51 _initialSensorBiasesApplied = false;
933 51 _heightBiasTotal = 0.0;
934 51 _heightScaleTotal = 1.0;
935
936
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 _kalmanFilter.setZero();
937
938 // Initial Covariance of the attitude angles in [rad²]
939
2/4
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
 51 Eigen::Vector3d variance_angles = Eigen::Vector3d::Zero();
940
1/5
✗ Branch 0 not taken.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 51 times.
✗ Branch 4 not taken.
 51 switch (_initCovarianceAttitudeAnglesUnit)
941 {
942 ✗ case InitCovarianceAttitudeAnglesUnit::rad2:
943 ✗ variance_angles = _initCovarianceAttitudeAngles;
944 ✗ break;
945 ✗ case InitCovarianceAttitudeAnglesUnit::deg2:
946 ✗ variance_angles = deg2rad(_initCovarianceAttitudeAngles);
947 ✗ break;
948 ✗ case InitCovarianceAttitudeAnglesUnit::rad:
949 ✗ variance_angles = _initCovarianceAttitudeAngles.array().pow(2);
950 ✗ break;
951 51 case InitCovarianceAttitudeAnglesUnit::deg:
952
4/8
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 51 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 51 times.
✗ Branch 11 not taken.
 51 variance_angles = deg2rad(_initCovarianceAttitudeAngles).array().pow(2);
953 51 break;
954 }
955
956 // Initial Covariance of the velocity in [m²/s²]
957
2/4
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
 51 Eigen::Vector3d variance_vel = Eigen::Vector3d::Zero();
958
1/3
✗ Branch 0 not taken.
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 switch (_initCovarianceVelocityUnit)
959 {
960 ✗ case InitCovarianceVelocityUnit::m2_s2:
961 ✗ variance_vel = _initCovarianceVelocity;
962 ✗ break;
963 51 case InitCovarianceVelocityUnit::m_s:
964
3/6
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 51 times.
✗ Branch 8 not taken.
 51 variance_vel = _initCovarianceVelocity.array().pow(2);
965 51 break;
966 }
967
968
2/4
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
 51 Eigen::Vector3d e_variance = Eigen::Vector3d::Zero(); // Initial Covariance of the position in [m²]
969
2/4
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
 51 Eigen::Vector3d lla_variance = Eigen::Vector3d::Zero(); // Initial Covariance of the position in [rad² rad² m²]
970
1/5
✗ Branch 0 not taken.
✗ Branch 1 not taken.
✓ Branch 2 taken 51 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 51 switch (_initCovariancePositionUnit)
971 {
972 ✗ case InitCovariancePositionUnit::rad2_rad2_m2:
973 ✗ e_variance = trafo::lla2ecef_WGS84(_initCovariancePosition.cwiseSqrt()).array().pow(2);
974 ✗ lla_variance = _initCovariancePosition;
975 ✗ break;
976 ✗ case InitCovariancePositionUnit::rad_rad_m:
977 ✗ e_variance = trafo::lla2ecef_WGS84(_initCovariancePosition).array().pow(2);
978 ✗ lla_variance = _initCovariancePosition.array().pow(2);
979 ✗ break;
980 51 case InitCovariancePositionUnit::meter:
981
3/6
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 51 times.
✗ Branch 8 not taken.
 51 e_variance = _initCovariancePosition.array().pow(2);
982
6/12
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 51 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 51 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 51 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 51 times.
✗ Branch 17 not taken.
 51 lla_variance = (trafo::ecef2lla_WGS84(trafo::ned2ecef(_initCovariancePosition, Eigen::Vector3d{ 0, 0, 0 }))).array().pow(2);
983 51 break;
984 ✗ case InitCovariancePositionUnit::meter2:
985 ✗ e_variance = _initCovariancePosition;
986 ✗ lla_variance = (trafo::ecef2lla_WGS84(trafo::ned2ecef(_initCovariancePosition.cwiseSqrt(), Eigen::Vector3d{ 0, 0, 0 }))).array().pow(2);
987 ✗ break;
988 }
989
990 // Initial Covariance of the accelerometer biases in [m^2/s^4]
991
2/4
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
 51 Eigen::Vector3d variance_accelBias = Eigen::Vector3d::Zero();
992
1/3
✗ Branch 0 not taken.
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 switch (_initCovarianceBiasAccelUnit)
993 {
994 ✗ case InitCovarianceBiasAccelUnit::m2_s4:
995 ✗ variance_accelBias = _initCovarianceBiasAccel;
996 ✗ break;
997 51 case InitCovarianceBiasAccelUnit::m_s2:
998
3/6
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 51 times.
✗ Branch 8 not taken.
 51 variance_accelBias = _initCovarianceBiasAccel.array().pow(2);
999 51 break;
1000 }
1001
1002 // Initial Covariance of the gyroscope biases in [rad^2/s^2]
1003
2/4
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
 51 Eigen::Vector3d variance_gyroBias = Eigen::Vector3d::Zero();
1004
2/5
✗ Branch 0 not taken.
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
✓ Branch 3 taken 48 times.
✗ Branch 4 not taken.
 51 switch (_initCovarianceBiasGyroUnit)
1005 {
1006 ✗ case InitCovarianceBiasGyroUnit::rad2_s2:
1007 ✗ variance_gyroBias = _initCovarianceBiasGyro;
1008 ✗ break;
1009 ✗ case InitCovarianceBiasGyroUnit::deg2_s2:
1010 ✗ variance_gyroBias = deg2rad(_initCovarianceBiasGyro.array().sqrt()).array().pow(2);
1011 ✗ break;
1012 3 case InitCovarianceBiasGyroUnit::rad_s:
1013
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 3 variance_gyroBias = _initCovarianceBiasGyro.array().pow(2);
1014 3 break;
1015 48 case InitCovarianceBiasGyroUnit::deg_s:
1016
4/8
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 48 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 48 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 48 times.
✗ Branch 11 not taken.
 48 variance_gyroBias = deg2rad(_initCovarianceBiasGyro).array().pow(2);
1017 48 break;
1018 }
1019
1020 // Initial Covariance of the height bias in [m^2]
1021 51 double variance_heightBias = 0.0;
1022
1/3
✗ Branch 0 not taken.
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 switch (_initCovarianceBiasHeightUnit)
1023 {
1024 ✗ case InitCovarianceBiasHeightUnit::m:
1025 ✗ variance_heightBias = std::pow(_initCovarianceBiasHeight, 2);
1026 ✗ break;
1027 51 case InitCovarianceBiasHeightUnit::m2:
1028 51 variance_heightBias = _initCovarianceBiasHeight;
1029 51 break;
1030 }
1031
1032 // Initial Covariance of the height scale in [m^2/m^2]
1033 51 double variance_heightScale = 0.0;
1034
1/3
✗ Branch 0 not taken.
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 switch (_initCovarianceScaleHeightUnit)
1035 {
1036 ✗ case InitCovarianceScaleHeight::m_m:
1037 ✗ variance_heightScale = std::pow(_initCovarianceScaleHeight, 2);
1038 ✗ break;
1039 51 case InitCovarianceScaleHeight::m2_m2:
1040 51 variance_heightScale = _initCovarianceScaleHeight;
1041 51 break;
1042 }
1043
1044 // 𝐏 Error covariance matrix
1045
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 102 _kalmanFilter.P = initialErrorCovarianceMatrix_P0(variance_angles, // Flight Angles covariance
1046 variance_vel, // Velocity covariance
1047
3/4
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 27 times.
✓ Branch 4 taken 24 times.
 51 _inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::NED
1048 ? lla_variance
1049 : e_variance, // Position (Lat, Lon, Alt) / ECEF covariance
1050 variance_accelBias, // Accelerometer Bias covariance
1051 variance_gyroBias, // Gyroscope Bias covariance
1052 variance_heightBias, // height bias covariance
1053 51 variance_heightScale); // height scale covariance
1054
1055 // Initial acceleration bias in body frame coordinates in [m/s^2]
1056
1/3
✓ Branch 0 taken 51 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 51 switch (_initBiasAccelUnit)
1057 {
1058 51 case InitBiasAccelUnit::microg:
1059
3/6
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 51 times.
✗ Branch 8 not taken.
 51 _accelBiasTotal = _initBiasAccel * 1e-6 * InsConst::G_NORM;
1060 51 break;
1061 ✗ case InitBiasAccelUnit::m_s2:
1062 ✗ _accelBiasTotal = _initBiasAccel;
1063 ✗ break;
1064 }
1065
1066 // Initial angular rate bias in body frame coordinates in [rad/s]
1067
1/3
✓ Branch 0 taken 51 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 51 switch (_initBiasGyroUnit)
1068 {
1069 51 case InitBiasGyroUnit::deg_s:
1070
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 _gyroBiasTotal = deg2rad(_initBiasGyro);
1071 51 break;
1072 ✗ case InitBiasGyroUnit::rad_s:
1073 ✗ _gyroBiasTotal = _initBiasGyro;
1074 ✗ break;
1075 }
1076
1077
3/6
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 51 times.
✗ Branch 9 not taken.
 51 LOG_DEBUG("{}: initialized", nameId());
1078 LOG_DATA("{}: P_0 =\n{}", nameId(), _kalmanFilter.P);
1079
1080 51 return true;
1081 }
1082
1083 17 void NAV::LooselyCoupledKF::deinitialize()
1084 {
1085 LOG_TRACE("{}: called", nameId());
1086 17 }
1087
1088 102603 bool NAV::LooselyCoupledKF::hasInputPinWithSameTime(const InsTime& insTime) const
1089 {
1090 102603 return std::ranges::any_of(inputPins, [&insTime](const InputPin& pin) {
1091
8/12
✓ Branch 1 taken 307809 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 102603 times.
✓ Branch 6 taken 205206 times.
✓ Branch 8 taken 102584 times.
✓ Branch 9 taken 19 times.
✓ Branch 13 taken 24238 times.
✓ Branch 14 taken 78346 times.
✓ Branch 15 taken 307809 times.
✗ Branch 16 not taken.
✗ Branch 18 not taken.
✗ Branch 19 not taken.
 307809 return pin.dataIdentifier.front() == PosVel::type() && !pin.queue.empty() && pin.queue.front()->insTime == insTime;
1092 102603 });
1093 }
1094
1095 25793 void NAV::LooselyCoupledKF::invokeCallbackWithPosVelAtt(const PosVelAtt& posVelAtt)
1096 {
1097
1/2
✓ Branch 1 taken 25793 times.
✗ Branch 2 not taken.
 25793 auto lckfSolution = std::make_shared<InsGnssLCKFSolution>();
1098 25793 lckfSolution->insTime = posVelAtt.insTime;
1099
2/2
✓ Branch 2 taken 25776 times.
✓ Branch 3 taken 17 times.
 25793 if (posVelAtt.e_CovarianceMatrix())
1100 {
1101
1/2
✓ Branch 3 taken 25776 times.
✗ Branch 4 not taken.
 51552 lckfSolution->setPosVelAttAndCov_e(posVelAtt.e_position(),
1102 25776 posVelAtt.e_velocity(),
1103 25776 posVelAtt.e_Quat_b(),
1104
1/2
✓ Branch 3 taken 25776 times.
✗ Branch 4 not taken.
 51552 (*posVelAtt.e_CovarianceMatrix())(all, all));
1105 }
1106 else
1107 {
1108
1/2
✓ Branch 5 taken 17 times.
✗ Branch 6 not taken.
 17 lckfSolution->setPosVelAtt_e(posVelAtt.e_position(), posVelAtt.e_velocity(), posVelAtt.e_Quat_b());
1109 }
1110
1111
1/2
✓ Branch 1 taken 25793 times.
✗ Branch 2 not taken.
 51586 lckfSolution->frame = _inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::NED
1112
2/2
✓ Branch 0 taken 12897 times.
✓ Branch 1 taken 12896 times.
 25793 ? InsGnssLCKFSolution::Frame::NED
1113 : InsGnssLCKFSolution::Frame::ECEF;
1114
1115
2/2
✓ Branch 1 taken 25776 times.
✓ Branch 2 taken 17 times.
 25793 if (_lastImuObs)
1116 {
1117
3/6
✓ Branch 1 taken 25776 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25776 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 25776 times.
✗ Branch 10 not taken.
 25776 lckfSolution->b_biasAccel.value = _lastImuObs->imuPos.b_quat_p() * -_inertialIntegrator.p_getLastAccelerationBias();
1118 25776 lckfSolution->b_biasAccel.stdDev = Eigen::Vector3d{
1119
1/2
✓ Branch 1 taken 25776 times.
✗ Branch 2 not taken.
 25776 std::sqrt(_kalmanFilter.P(KFStates::AccBiasX, KFStates::AccBiasX) * (1. / std::pow(SCALE_FACTOR_ACCELERATION, 2))),
1120
1/2
✓ Branch 1 taken 25776 times.
✗ Branch 2 not taken.
 25776 std::sqrt(_kalmanFilter.P(KFStates::AccBiasY, KFStates::AccBiasY) * (1. / std::pow(SCALE_FACTOR_ACCELERATION, 2))),
1121
2/4
✓ Branch 1 taken 25776 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 25776 times.
✗ Branch 6 not taken.
 25776 std::sqrt(_kalmanFilter.P(KFStates::AccBiasZ, KFStates::AccBiasZ) * (1. / std::pow(SCALE_FACTOR_ACCELERATION, 2)))
1122 25776 };
1123
3/6
✓ Branch 1 taken 25776 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25776 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 25776 times.
✗ Branch 10 not taken.
 25776 lckfSolution->b_biasGyro.value = _lastImuObs->imuPos.b_quat_p() * -_inertialIntegrator.p_getLastAngularRateBias();
1124 25776 lckfSolution->b_biasGyro.stdDev = Eigen::Vector3d{
1125
1/2
✓ Branch 1 taken 25776 times.
✗ Branch 2 not taken.
 25776 std::sqrt(_kalmanFilter.P(KFStates::GyrBiasX, KFStates::GyrBiasX) * (1. / std::pow(SCALE_FACTOR_ANGULAR_RATE, 2))),
1126
1/2
✓ Branch 1 taken 25776 times.
✗ Branch 2 not taken.
 25776 std::sqrt(_kalmanFilter.P(KFStates::GyrBiasY, KFStates::GyrBiasY) * (1. / std::pow(SCALE_FACTOR_ANGULAR_RATE, 2))),
1127
2/4
✓ Branch 1 taken 25776 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 25776 times.
✗ Branch 6 not taken.
 51552 std::sqrt(_kalmanFilter.P(KFStates::GyrBiasZ, KFStates::GyrBiasZ) * (1. / std::pow(SCALE_FACTOR_ANGULAR_RATE, 2)))
1128 25776 };
1129 }
1130
1/2
✓ Branch 2 taken 25793 times.
✗ Branch 3 not taken.
 25793 lckfSolution->heightBias = { .value = _heightBiasTotal, .stdDev = std::sqrt(_kalmanFilter.P(KFStates::HeightBias, KFStates::HeightBias)) };
1131
1/2
✓ Branch 2 taken 25793 times.
✗ Branch 3 not taken.
 25793 lckfSolution->heightScale = { .value = _heightScaleTotal, .stdDev = std::sqrt(_kalmanFilter.P(KFStates::HeightScale, KFStates::HeightScale)) };
1132
3/4
✓ Branch 2 taken 25793 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 25776 times.
✓ Branch 5 taken 17 times.
 25793 if (!hasInputPinWithSameTime(lckfSolution->insTime))
1133 {
1134
1/2
✓ Branch 2 taken 25776 times.
✗ Branch 3 not taken.
 25776 invokeCallbacks(OUTPUT_PORT_INDEX_SOLUTION, lckfSolution);
1135 }
1136 25793 }
1137
1138 49998 void NAV::LooselyCoupledKF::recvImuObservation(InputPin::NodeDataQueue& queue, size_t /* pinIdx */)
1139 {
1140
1/2
✓ Branch 1 taken 49998 times.
✗ Branch 2 not taken.
 49998 auto nodeData = queue.extract_front();
1141
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 49998 times.
 49998 if (nodeData->insTime.empty())
1142 {
1143 ✗ LOG_ERROR("{}: Can't set new imuObs__t0 because the observation has no time tag (insTime)", nameId());
1144 ✗ return;
1145 }
1146
1147 49998 std::shared_ptr<NAV::PosVelAtt> inertialNavSol = nullptr;
1148
1149 49998 _lastImuObs = std::static_pointer_cast<const ImuObs>(nodeData);
1150
1151 99996 if (!_preferAccelerationOverDeltaMeasurements
1152
5/30
✗ Branch 0 not taken.
✓ Branch 1 taken 49998 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✓ Branch 17 taken 49998 times.
✗ Branch 19 not taken.
✓ Branch 20 taken 49998 times.
✗ Branch 21 not taken.
✓ Branch 22 taken 49998 times.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 49998 times.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
 49998 && NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(inputPins.at(INPUT_PORT_INDEX_IMU).link.getConnectedPin()->dataIdentifier, { ImuObsWDelta::type() }))
1153 {
1154 ✗ auto obs = std::static_pointer_cast<const ImuObsWDelta>(nodeData);
1155 LOG_DATA("{}: [{}] recvImuObsWDelta", nameId(), obs->insTime.toYMDHMS(GPST));
1156
1157 // Initialize biases
1158 ✗ if (!_initialSensorBiasesApplied)
1159 {
1160 ✗ _inertialIntegrator.setTotalSensorBiases(obs->imuPos.p_quat_b() * -_accelBiasTotal, obs->imuPos.p_quat_b() * -_gyroBiasTotal);
1161 ✗ _initialSensorBiasesApplied = true;
1162 }
1163
1164 ✗ inertialNavSol = _inertialIntegrator.calcInertialSolutionDelta(obs->insTime, obs->dtime, obs->dvel, obs->dtheta, obs->imuPos, nameId().c_str());
1165 ✗ }
1166 else
1167 {
1168 49998 auto obs = std::static_pointer_cast<const ImuObs>(nodeData);
1169 LOG_DATA("{}: [{}] recvImuObs", nameId(), obs->insTime.toYMDHMS(GPST));
1170
1171 // Initialize biases
1172
2/2
✓ Branch 0 taken 17 times.
✓ Branch 1 taken 49981 times.
 49998 if (!_initialSensorBiasesApplied)
1173 {
1174
7/14
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 17 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 17 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 17 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 17 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 17 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 17 times.
✗ Branch 22 not taken.
 17 _inertialIntegrator.setTotalSensorBiases(obs->imuPos.p_quat_b() * -_accelBiasTotal, obs->imuPos.p_quat_b() * -_gyroBiasTotal);
1175 17 _initialSensorBiasesApplied = true;
1176 }
1177
1178
2/4
✓ Branch 1 taken 49998 times.
✗ Branch 2 not taken.
✓ Branch 9 taken 49998 times.
✗ Branch 10 not taken.
 49998 inertialNavSol = _inertialIntegrator.calcInertialSolution(obs->insTime, obs->p_acceleration, obs->p_angularRate, obs->imuPos, nameId().c_str());
1179 49998 }
1180
1/2
✓ Branch 1 taken 49998 times.
✗ Branch 2 not taken.
 49998 if (const auto& latestState = _inertialIntegrator.getLatestState();
1181
6/8
✓ Branch 1 taken 49998 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49998 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 49997 times.
✓ Branch 9 taken 1 times.
✓ Branch 10 taken 49997 times.
✓ Branch 11 taken 1 times.
 49998 inertialNavSol && latestState && latestState->get().m.dt > 1e-8)
1182 {
1183
1/2
✓ Branch 6 taken 49997 times.
✗ Branch 7 not taken.
 49997 looselyCoupledPrediction(inertialNavSol, latestState->get().m.dt, std::static_pointer_cast<const ImuObs>(nodeData)->imuPos);
1184
1185
3/6
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 49997 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 49997 times.
✗ Branch 10 not taken.
 99994 setSolutionPosVelAttAndCov(inertialNavSol,
1186 49997 calcEarthRadius_N(inertialNavSol->latitude()),
1187 49997 calcEarthRadius_E(inertialNavSol->latitude()));
1188
1189 LOG_DATA("{}: e_position = {}", nameId(), inertialNavSol->e_position().transpose());
1190 LOG_DATA("{}: e_velocity = {}", nameId(), inertialNavSol->e_velocity().transpose());
1191 LOG_DATA("{}: rollPitchYaw = {}", nameId(), rad2deg(inertialNavSol->rollPitchYaw()).transpose());
1192
1193
3/4
✓ Branch 2 taken 49997 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 25776 times.
✓ Branch 5 taken 24221 times.
 49997 if (!hasInputPinWithSameTime(nodeData->insTime))
1194 {
1195 LOG_DATA("{}: [{}] Sending out predicted solution", nameId(), inertialNavSol->insTime.toYMDHMS(GPST));
1196
1/2
✓ Branch 2 taken 25776 times.
✗ Branch 3 not taken.
 25776 invokeCallbackWithPosVelAtt(*inertialNavSol);
1197 }
1198 }
1199 49998 }
1200
1201 26830 void NAV::LooselyCoupledKF::recvPosVelObservation(InputPin::NodeDataQueue& queue, size_t /* pinIdx */)
1202 {
1203
1/2
✓ Branch 1 taken 26830 times.
✗ Branch 2 not taken.
 26830 auto obs = std::static_pointer_cast<const PosVel>(queue.extract_front());
1204 LOG_DATA("{}: [{}] recvPosVelObservation", nameId(), obs->insTime.toYMDHMS(GPST));
1205
1206
2/8
✗ Branch 0 not taken.
✓ Branch 1 taken 26830 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 26830 times.
 26830 if (!_initializeStateOverExternalPin && !_inertialIntegrator.hasInitialPosition())
1207 {
1208 ✗ for (size_t i = _dynamicInputPins.getFirstDynamicPinIdx(); i < _dynamicInputPins.getFirstDynamicPinIdx() + _dynamicInputPins.getNumberOfDynamicPins(); i++)
1209 {
1210 ✗ inputPins.at(i).priority = 0; // IMU obs (prediction) should be evaluated before the PosVel obs (update)
1211 }
1212
1213 ✗ PosVelAtt posVelAtt;
1214 ✗ posVelAtt.insTime = obs->insTime;
1215 ✗ posVelAtt.setPosVelAtt_n(obs->lla_position(), obs->n_velocity(),
1216 ✗ trafo::n_Quat_b(deg2rad(_initalRollPitchYaw[0]), deg2rad(_initalRollPitchYaw[1]), deg2rad(_initalRollPitchYaw[2])));
1217
1218 ✗ _inertialIntegrator.setInitialState(posVelAtt, nameId().c_str());
1219 LOG_DATA("{}: e_position = {}", nameId(), posVelAtt.e_position().transpose());
1220 LOG_DATA("{}: e_velocity = {}", nameId(), posVelAtt.e_velocity().transpose());
1221 LOG_DATA("{}: rollPitchYaw = {}", nameId(), rad2deg(posVelAtt.rollPitchYaw()).transpose());
1222
1223 LOG_DATA("{}: [{}] Sending out initial solution", nameId(), obs->insTime.toYMDHMS(GPST));
1224 ✗ invokeCallbackWithPosVelAtt(posVelAtt);
1225 ✗ return;
1226 ✗ }
1227
1228
2/2
✓ Branch 2 taken 17 times.
✓ Branch 3 taken 26813 times.
 26830 if (_externalInitTime == obs->insTime) { return; }
1229
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 26813 times.
 26813 if (!_lastImuObs)
1230 {
1231 ✗ PosVelAtt posVelAtt;
1232 ✗ posVelAtt.insTime = obs->insTime;
1233 ✗ if (obs->n_CovarianceMatrix())
1234 {
1235 ✗ Eigen::Matrix<double, 10, 10> n_cov = Eigen::Matrix<double, 10, 10>::Zero();
1236 ✗ n_cov.topLeftCorner<6, 6>() = obs->n_CovarianceMatrix()->get()(Keys::PosVel<Keys::MotionModelKey>, Keys::PosVel<Keys::MotionModelKey>);
1237 ✗ posVelAtt.setPosVelAttAndCov_n(obs->lla_position(), obs->n_velocity(), _inertialIntegrator.getLatestState()->get().attitude, n_cov);
1238 }
1239 ✗ else { posVelAtt.setPosVelAtt_n(obs->lla_position(), obs->n_velocity(), _inertialIntegrator.getLatestState()->get().attitude); }
1240 ✗ _inertialIntegrator.addState(posVelAtt, nameId().c_str());
1241
1242 LOG_DATA("{}: [{}] Sending out received solution, as no IMU data yet", nameId(), obs->insTime.toYMDHMS(GPST));
1243 ✗ invokeCallbackWithPosVelAtt(posVelAtt);
1244 ✗ return;
1245 ✗ }
1246
1247
1/2
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
 26813 looselyCoupledUpdate(obs);
1248 26830 }
1249
1250 ✗ void NAV::LooselyCoupledKF::recvBaroHeight([[maybe_unused]] InputPin::NodeDataQueue& queue, size_t /* pinIdx */)
1251 {
1252 ✗ auto obs = std::static_pointer_cast<const BaroHgt>(queue.extract_front());
1253 LOG_DATA("{}: [{}] recvPosVelObservation", nameId(), obs->insTime.toYMDHMS(GPST));
1254
1255 ✗ if (!_inertialIntegrator.hasInitialPosition())
1256 {
1257 ✗ return;
1258 }
1259
1260 ✗ if (_externalInitTime == obs->insTime) { return; }
1261 ✗ if (!_lastImuObs)
1262 {
1263 ✗ return;
1264 }
1265
1266 ✗ looselyCoupledUpdate(obs);
1267 ✗ }
1268
1269 17 void NAV::LooselyCoupledKF::recvPosVelAttInit(InputPin::NodeDataQueue& queue, size_t /* pinIdx */)
1270 {
1271
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 auto posVelAtt = std::static_pointer_cast<const PosVelAtt>(queue.extract_front());
1272
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 inputPins.at(INPUT_PORT_INDEX_POS_VEL_ATT_INIT).queueBlocked = true;
1273
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 inputPins.at(INPUT_PORT_INDEX_POS_VEL_ATT_INIT).queue.clear();
1274
1275 LOG_DATA("{}: recvPosVelAttInit at time [{}]", nameId(), posVelAtt->insTime.toYMDHMS());
1276
1277
3/4
✓ Branch 3 taken 34 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 17 times.
✓ Branch 6 taken 17 times.
 34 for (size_t i = _dynamicInputPins.getFirstDynamicPinIdx(); i < _dynamicInputPins.getFirstDynamicPinIdx() + _dynamicInputPins.getNumberOfDynamicPins(); i++)
1278 {
1279
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 inputPins.at(i).priority = 0; // IMU obs (prediction) should be evaluated before the PosVel obs (update)
1280 }
1281 17 _externalInitTime = posVelAtt->insTime;
1282
1283
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 17 times.
✗ Branch 7 not taken.
 17 _inertialIntegrator.setInitialState(*posVelAtt, nameId().c_str());
1284 LOG_DATA("{}: e_position = {}", nameId(), posVelAtt->e_position().transpose());
1285 LOG_DATA("{}: e_velocity = {}", nameId(), posVelAtt->e_velocity().transpose());
1286 LOG_DATA("{}: rollPitchYaw = {}", nameId(), rad2deg(posVelAtt->rollPitchYaw()).transpose());
1287
1288
1/2
✓ Branch 2 taken 17 times.
✗ Branch 3 not taken.
 17 invokeCallbackWithPosVelAtt(*posVelAtt);
1289 17 }
1290
1291 // ###########################################################################################################
1292 // Kalman Filter
1293 // ###########################################################################################################
1294
1295 49997 void NAV::LooselyCoupledKF::looselyCoupledPrediction(const std::shared_ptr<const PosVelAtt>& inertialNavSol, double tau_i, const ImuPos& imuPos)
1296 {
1297 LOG_DATA("{}: [{}] Predicting", nameId(), inertialNavSol->insTime.toYMDHMS(GPST));
1298
1299 // ------------------------------------------- GUI Parameters ----------------------------------------------
1300
1301 // 𝜎_ra Standard deviation of the noise on the accelerometer specific-force state [m / (s^2 · √(s))]
1302
1/2
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
 49997 Eigen::Vector3d sigma_ra = convertUnit(_stdev_ra, _stdevAccelNoiseUnits);
1303 LOG_DATA("{}: sigma_ra = {} [m / (s^2 · √(s))]", nameId(), sigma_ra.transpose());
1304
1305 // 𝜎_rg Standard deviation of the noise on the gyro angular-rate state [rad / (s · √(s))]
1306
1/2
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
 49997 Eigen::Vector3d sigma_rg = convertUnit(_stdev_rg, _stdevGyroNoiseUnits);
1307 LOG_DATA("{}: sigma_rg = {} [rad / (s · √(s))]", nameId(), sigma_rg.transpose());
1308
1309 // 𝜎_bad Standard deviation of the accelerometer dynamic bias [m / s^2]
1310
1/2
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
 49997 Eigen::Vector3d sigma_bad = convertUnit(_stdev_bad, _stdevAccelBiasUnits);
1311 LOG_DATA("{}: sigma_bad = {} [m / s^2]", nameId(), sigma_bad.transpose());
1312
1313 // 𝜎_bgd Standard deviation of the gyro dynamic bias [rad / s]
1314
1/2
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
 49997 Eigen::Vector3d sigma_bgd = convertUnit(_stdev_bgd, _stdevGyroBiasUnits);
1315 LOG_DATA("{}: sigma_bgd = {} [rad / s]", nameId(), sigma_bgd.transpose());
1316
1317 // Standard deviation of the barometric height bias [m]
1318 49997 double sigma_heightBias{};
1319
1/2
✓ Branch 0 taken 49997 times.
✗ Branch 1 not taken.
 49997 switch (_stdevBaroHeightBiasUnits)
1320 {
1321 49997 case StdevBaroHeightBiasUnits::m: // [m]
1322 49997 sigma_heightBias = _stdevBaroHeightBias;
1323 49997 break;
1324 }
1325 LOG_DATA("{}: sigma_heightBias = {} [m]", nameId(), sigma_heightBias);
1326
1327 // Standard deviation of the barometric height scale [m/m]
1328 49997 double sigma_heightScale{};
1329
1/2
✓ Branch 0 taken 49997 times.
✗ Branch 1 not taken.
 49997 switch (_stdevBaroHeightScaleUnits)
1330 {
1331 49997 case StdevBaroHeightScaleUnits::m_m: // [m/m]
1332 49997 sigma_heightScale = _stdevBaroHeightScale;
1333 49997 break;
1334 }
1335
1336 // ---------------------------------------------- Prediction -------------------------------------------------
1337
1338 // Latitude 𝜙, longitude λ and altitude (height above ground) in [rad, rad, m] at the time tₖ₋₁
1339 49997 const Eigen::Vector3d& lla_position = inertialNavSol->lla_position();
1340 LOG_DATA("{}: lla_position = {} [rad, rad, m]", nameId(), lla_position.transpose());
1341 // Prime vertical radius of curvature (East/West) [m]
1342
1/2
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
 49997 double R_E = calcEarthRadius_E(lla_position(0));
1343 LOG_DATA("{}: R_E = {} [m]", nameId(), R_E);
1344 // Geocentric Radius in [m]
1345
1/2
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
 49997 double r_eS_e = calcGeocentricRadius(lla_position(0), R_E);
1346 LOG_DATA("{}: r_eS_e = {} [m]", nameId(), r_eS_e);
1347
1348
1/2
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
 49997 auto p_acceleration = _inertialIntegrator.p_calcCurrentAcceleration();
1349 // Acceleration in [m/s^2], in body coordinates
1350 49997 Eigen::Vector3d b_acceleration = p_acceleration
1351
2/4
✓ Branch 0 taken 49997 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 49997 times.
✗ Branch 4 not taken.
 49997 ? imuPos.b_quat_p() * p_acceleration.value()
1352
1/6
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 49997 : Eigen::Vector3d::Zero();
1353 LOG_DATA("{}: b_acceleration = {} [m/s^2]", nameId(), b_acceleration.transpose());
1354
1355
3/4
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 37109 times.
✓ Branch 4 taken 12888 times.
 49997 if (_inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::NED)
1356 {
1357 // n_velocity (tₖ₋₁) Velocity in [m/s], in navigation coordinates, at the time tₖ₋₁
1358 37109 const Eigen::Vector3d& n_velocity = inertialNavSol->n_velocity();
1359 LOG_DATA("{}: n_velocity = {} [m / s]", nameId(), n_velocity.transpose());
1360 // q (tₖ₋₁) Quaternion, from body to navigation coordinates, at the time tₖ₋₁
1361 37109 const Eigen::Quaterniond& n_Quat_b = inertialNavSol->n_Quat_b();
1362 LOG_DATA("{}: n_Quat_b --> Roll, Pitch, Yaw = {} [deg]", nameId(), deg2rad(trafo::quat2eulerZYX(n_Quat_b).transpose()));
1363
1364 // Meridian radius of curvature in [m]
1365
1/2
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
 37109 double R_N = calcEarthRadius_N(lla_position(0));
1366 LOG_DATA("{}: R_N = {} [m]", nameId(), R_N);
1367
1368 // Conversion matrix between cartesian and curvilinear perturbations to the position
1369
1/2
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
 37109 Eigen::Matrix3d T_rn_p = conversionMatrixCartesianCurvilinear(lla_position, R_N, R_E);
1370 LOG_DATA("{}: T_rn_p =\n{}", nameId(), T_rn_p);
1371
1372 // Gravitation at surface level in [m/s^2]
1373
2/4
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.
 37109 double g_0 = n_calcGravitation_EGM96(lla_position).norm();
1374
1375 // omega_in^n = omega_ie^n + omega_en^n
1376
2/4
✓ Branch 2 taken 37109 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 37109 times.
✗ Branch 6 not taken.
 37109 Eigen::Vector3d n_omega_in = inertialNavSol->n_Quat_e() * InsConst::e_omega_ie
1377
3/6
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 37109 times.
✗ Branch 8 not taken.
 74218 + n_calcTransportRate(lla_position, n_velocity, R_N, R_E);
1378 LOG_DATA("{}: n_omega_in = {} [rad/s]", nameId(), n_omega_in.transpose());
1379
1380 // System Matrix
1381
1/2
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
 37109 _kalmanFilter.F = n_systemMatrix_F(n_Quat_b, b_acceleration, n_omega_in, n_velocity, lla_position, R_N, R_E, g_0, r_eS_e, _tau_bad, _tau_bgd);
1382 LOG_DATA("{}: F =\n{}", nameId(), _kalmanFilter.F);
1383
2/2
✓ Branch 0 taken 6444 times.
✓ Branch 1 taken 30665 times.
 37109 if (_qCalculationAlgorithm == QCalculationAlgorithm::Taylor1)
1384 {
1385 // 2. Calculate the system noise covariance matrix Q_{k-1}
1386
10/20
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6444 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6444 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6444 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 6444 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 6444 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 6444 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 6444 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 6444 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 6444 times.
✗ Branch 29 not taken.
 25776 _kalmanFilter.Q = n_systemNoiseCovarianceMatrix_Q(sigma_ra.array().square(), sigma_rg.array().square(),
1387
3/6
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6444 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6444 times.
✗ Branch 8 not taken.
 12888 sigma_bad.array().square(), sigma_bgd.array().square(),
1388 sigma_heightBias, sigma_heightScale,
1389
1/2
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
 6444 _tau_bad, _tau_bgd,
1390
1/2
✓ Branch 3 taken 6444 times.
✗ Branch 4 not taken.
 6444 _kalmanFilter.F.block<3>(KFVel, KFAtt), T_rn_p,
1391
1/2
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
 19332 n_Quat_b.toRotationMatrix(), tau_i);
1392 }
1393 }
1394 else // if (_inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::ECEF)
1395 {
1396 // e_position (tₖ₋₁) Position in [m/s], in ECEF coordinates, at the time tₖ₋₁
1397 12888 const Eigen::Vector3d& e_position = inertialNavSol->e_position();
1398 LOG_DATA("{}: e_position = {} [m]", nameId(), e_position.transpose());
1399 // q (tₖ₋₁) Quaternion, from body to Earth coordinates, at the time tₖ₋₁
1400 12888 const Eigen::Quaterniond& e_Quat_b = inertialNavSol->e_Quat_b();
1401 LOG_DATA("{}: e_Quat_b = {}", nameId(), e_Quat_b);
1402
1403 // Gravitation in [m/s^2] in ECEF coordinates
1404
5/10
✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12888 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12888 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12888 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 12888 times.
✗ Branch 14 not taken.
 12888 Eigen::Vector3d e_gravitation = trafo::e_Quat_n(lla_position(0), lla_position(1)) * n_calcGravitation_EGM96(lla_position);
1405
1406 // System Matrix
1407
1/2
✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
 12888 _kalmanFilter.F = e_systemMatrix_F(e_Quat_b, b_acceleration, e_position, e_gravitation, r_eS_e, InsConst::e_omega_ie, _tau_bad, _tau_bgd);
1408 LOG_DATA("{}: F =\n{}", nameId(), _kalmanFilter.F);
1409
2/2
✓ Branch 0 taken 6444 times.
✓ Branch 1 taken 6444 times.
 12888 if (_qCalculationAlgorithm == QCalculationAlgorithm::Taylor1)
1410 {
1411 // 2. Calculate the system noise covariance matrix Q_{k-1}
1412
10/20
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6444 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6444 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6444 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 6444 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 6444 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 6444 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 6444 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 6444 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 6444 times.
✗ Branch 29 not taken.
 25776 _kalmanFilter.Q = e_systemNoiseCovarianceMatrix_Q(sigma_ra.array().square(), sigma_rg.array().square(),
1413
3/6
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6444 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6444 times.
✗ Branch 8 not taken.
 12888 sigma_bad.array().square(), sigma_bgd.array().square(),
1414 sigma_heightBias, sigma_heightScale,
1415
1/2
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
 6444 _tau_bad, _tau_bgd,
1416
1/2
✓ Branch 3 taken 6444 times.
✗ Branch 4 not taken.
 6444 _kalmanFilter.F.block<3>(KFVel, KFAtt),
1417
1/2
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
 19332 e_Quat_b.toRotationMatrix(), tau_i);
1418 }
1419 }
1420
2/2
✓ Branch 0 taken 37109 times.
✓ Branch 1 taken 12888 times.
 49997 if (_qCalculationAlgorithm == QCalculationAlgorithm::VanLoan)
1421 {
1422 // Noise Input Matrix
1423
4/6
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 30665 times.
✓ Branch 4 taken 6444 times.
✓ Branch 6 taken 37109 times.
✗ Branch 7 not taken.
 111327 _kalmanFilter.G = noiseInputMatrix_G(_inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::NED
1424 30665 ? inertialNavSol->n_Quat_b()
1425 43553 : inertialNavSol->e_Quat_b());
1426 LOG_DATA("{}: G =\n{}", nameId(), _kalmanFilter.G);
1427
1428 37109 _kalmanFilter.W(all, all) = noiseScaleMatrix_W(sigma_ra, sigma_rg,
1429 sigma_bad, sigma_bgd,
1430
1/2
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
 37109 _tau_bad, _tau_bgd,
1431
1/2
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
 37109 sigma_heightBias, sigma_heightScale);
1432 LOG_DATA("{}: W =\n{}", nameId(), _kalmanFilter.W(all, all));
1433 LOG_DATA("{}: G*W*G^T =\n{}", nameId(), _kalmanFilter.G(all, all) * _kalmanFilter.W(all, all) * _kalmanFilter.G(all, all).transpose());
1434
1435 // 1. Calculate the transition matrix 𝚽_{k-1}
1436 // 2. Calculate the system noise covariance matrix Q_{k-1}
1437
1/2
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
 37109 _kalmanFilter.calcPhiAndQWithVanLoanMethod(tau_i);
1438 }
1439 // If Q was calculated over Van Loan, then the Phi matrix was automatically calculated with the exponential matrix
1440
4/4
✓ Branch 0 taken 37109 times.
✓ Branch 1 taken 12888 times.
✓ Branch 2 taken 6444 times.
✓ Branch 3 taken 30665 times.
 49997 if (_phiCalculationAlgorithm != PhiCalculationAlgorithm::Exponential || _qCalculationAlgorithm != QCalculationAlgorithm::VanLoan)
1441 {
1442 19332 auto calcPhi = [&]() {
1443
2/2
✓ Branch 0 taken 6444 times.
✓ Branch 1 taken 12888 times.
 19332 if (_phiCalculationAlgorithm == PhiCalculationAlgorithm::Exponential)
1444 {
1445 // 1. Calculate the transition matrix 𝚽_{k-1}
1446 6444 _kalmanFilter.calcTransitionMatrix_Phi_exp(tau_i);
1447 }
1448
1/2
✓ Branch 0 taken 12888 times.
✗ Branch 1 not taken.
 12888 else if (_phiCalculationAlgorithm == PhiCalculationAlgorithm::Taylor)
1449 {
1450 // 1. Calculate the transition matrix 𝚽_{k-1}
1451 12888 _kalmanFilter.calcTransitionMatrix_Phi_Taylor(tau_i, static_cast<size_t>(_phiCalculationTaylorOrder));
1452 }
1453 else
1454 {
1455 ✗ LOG_CRITICAL("{}: Calculation algorithm '{}' for the system matrix Phi is not supported.", nameId(), fmt::underlying(_phiCalculationAlgorithm));
1456 }
1457 19332 };
1458
1/2
✓ Branch 1 taken 19332 times.
✗ Branch 2 not taken.
 19332 calcPhi();
1459 }
1460 LOG_DATA("{}: KF.Phi =\n{}", nameId(), _kalmanFilter.Phi);
1461 LOG_DATA("{}: KF.Q =\n{}", nameId(), _kalmanFilter.Q);
1462
1463 LOG_DATA("{}: Q - Q^T =\n{}", nameId(), _kalmanFilter.Q(all, all) - _kalmanFilter.Q(all, all).transpose());
1464 LOG_DATA("{}: KF.P (before prediction) =\n{}", nameId(), _kalmanFilter.P);
1465
1466 // 3. Propagate the state vector estimate from x(+) and x(-)
1467 // 4. Propagate the error covariance matrix from P(+) and P(-)
1468
1/2
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
 49997 _kalmanFilter.predict();
1469
1470 LOG_DATA("{}: KF.x = {}", nameId(), _kalmanFilter.x.transposed());
1471 LOG_DATA("{}: KF.P (after prediction) =\n{}", nameId(), _kalmanFilter.P);
1472
1473 // Averaging of P to avoid numerical problems with symmetry (did not work)
1474 // _kalmanFilter.P = ((_kalmanFilter.P + _kalmanFilter.P.transpose()) / 2.0);
1475
1476 // LOG_DEBUG("{}: F\n{}\n", nameId(), F);
1477 // LOG_DEBUG("{}: Phi\n{}\n", nameId(), _kalmanFilter.Phi);
1478
1479 // LOG_DEBUG("{}: Q\n{}\n", nameId(), _kalmanFilter.Q);
1480 // LOG_DEBUG("{}: Q - Q^T\n{}\n", nameId(), _kalmanFilter.Q - _kalmanFilter.Q.transpose());
1481
1482 // LOG_DEBUG("{}: x\n{}\n", nameId(), _kalmanFilter.x);
1483
1484 // LOG_DEBUG("{}: P\n{}\n", nameId(), _kalmanFilter.P);
1485 // LOG_DEBUG("{}: P - P^T\n{}\n", nameId(), _kalmanFilter.P - _kalmanFilter.P.transpose());
1486
1487
1/2
✓ Branch 0 taken 49997 times.
✗ Branch 1 not taken.
 49997 if (_checkKalmanMatricesRanks)
1488 {
1489
1/2
✓ Branch 2 taken 49997 times.
✗ Branch 3 not taken.
 49997 Eigen::FullPivLU<Eigen::MatrixXd> lu(_kalmanFilter.P(all, all));
1490
1/2
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
 49997 auto rank = lu.rank();
1491
2/2
✓ Branch 2 taken 4273 times.
✓ Branch 3 taken 45724 times.
 49997 if (rank != _kalmanFilter.P(all, all).rows())
1492 {
1493
4/8
✓ Branch 1 taken 4273 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 4273 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 4273 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 4273 times.
✗ Branch 14 not taken.
 4273 LOG_WARN("{}: [{}] P.rank = {}", nameId(), inertialNavSol->insTime.toYMDHMS(GPST), rank);
1494 }
1495 49997 }
1496 49997 }
1497
1498 26813 void NAV::LooselyCoupledKF::looselyCoupledUpdate(const std::shared_ptr<const PosVel>& posVelObs)
1499 {
1500
2/4
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26813 times.
✗ Branch 5 not taken.
 26813 INS_ASSERT_USER_ERROR(_inertialIntegrator.getLatestState().has_value(), "The update should not even trigger without an initial state.");
1501
1502 LOG_DATA("{}: [{}] Updating (lastInertial at [{}])", nameId(), posVelObs->insTime.toYMDHMS(GPST),
1503 _inertialIntegrator.getLatestState().value().get().epoch.toYMDHMS(GPST));
1504
1505 // -------------------------------------------- GUI Parameters -----------------------------------------------
1506
1507 // Latitude 𝜙, longitude λ and altitude (height above ground) in [rad, rad, m] at the time tₖ₋₁
1508
1/2
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
 26813 Eigen::Vector3d lla_position;
1509
3/4
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 25517 times.
✓ Branch 4 taken 1296 times.
 26813 if (_inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::NED)
1510 {
1511
3/6
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 25517 times.
✗ Branch 9 not taken.
 25517 lla_position = _inertialIntegrator.getLatestState().value().get().position;
1512 }
1513 else // if (_inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::ECEF)
1514 {
1515
3/6
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1296 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1296 times.
✗ Branch 9 not taken.
 1296 lla_position = trafo::lla2ecef_WGS84(_inertialIntegrator.getLatestState().value().get().position);
1516 }
1517 LOG_DATA("{}: lla_position = {} [rad, rad, m]", nameId(), lla_position.transpose());
1518
1519 // ---------------------------------------------- Correction -------------------------------------------------
1520
1521
1/2
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
 26813 auto p_omega_ip = _inertialIntegrator.p_calcCurrentAngularRate();
1522 // Angular rate measured in units of [rad/s], and given in the body frame
1523 26813 Eigen::Vector3d b_omega_ip = p_omega_ip
1524
2/4
✓ Branch 0 taken 26813 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 26813 times.
✗ Branch 4 not taken.
 26813 ? _lastImuObs->imuPos.b_quat_p() * p_omega_ip.value()
1525
1/6
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 26813 : Eigen::Vector3d::Zero();
1526 LOG_DATA("{}: b_omega_ip = {} [rad/s]", nameId(), b_omega_ip.transpose());
1527
1528
1/2
✓ Branch 2 taken 26813 times.
✗ Branch 3 not taken.
 26813 _kalmanFilter.setMeasurements(Meas);
1529
1530 // Prime vertical radius of curvature (East/West) [m]
1531 26813 double R_E = 0.0;
1532 // Meridian radius of curvature in [m]
1533 26813 double R_N = 0.0;
1534
1535
1/2
✓ Branch 3 taken 26813 times.
✗ Branch 4 not taken.
 26813 Eigen::Vector3d b_leverArm_InsGnss = _lastImuObs->imuPos.b_positionIMU_p();
1536
1537
3/4
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 25517 times.
✓ Branch 4 taken 1296 times.
 26813 if (_inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::NED)
1538 {
1539
1/2
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
 25517 R_E = calcEarthRadius_E(lla_position(0));
1540 LOG_DATA("{}: R_E = {} [m]", nameId(), R_E);
1541
1/2
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
 25517 R_N = calcEarthRadius_N(lla_position(0));
1542 LOG_DATA("{}: R_N = {} [m]", nameId(), R_N);
1543
1544 // Direction Cosine Matrix from body to navigation coordinates, at the time tₖ₋₁
1545
3/6
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 25517 times.
✗ Branch 9 not taken.
 25517 Eigen::Matrix3d n_Dcm_b = _inertialIntegrator.getLatestState().value().get().attitude.toRotationMatrix();
1546 LOG_DATA("{}: n_Dcm_b =\n{}", nameId(), n_Dcm_b);
1547
1548 // Conversion matrix between cartesian and curvilinear perturbations to the position
1549
1/2
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
 25517 Eigen::Matrix3d T_rn_p = conversionMatrixCartesianCurvilinear(lla_position, R_N, R_E);
1550 LOG_DATA("{}: T_rn_p =\n{}", nameId(), T_rn_p);
1551
1552 // Skew-symmetric matrix of the Earth-rotation vector in local navigation frame axes
1553
4/8
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 25517 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 25517 times.
✗ Branch 12 not taken.
 25517 Eigen::Matrix3d n_Omega_ie = math::skewSymmetricMatrix(_inertialIntegrator.getLatestState().value().get().attitude * InsConst::e_omega_ie);
1554 LOG_DATA("{}: n_Omega_ie =\n{}", nameId(), n_Omega_ie);
1555
1556 // 5. Calculate the measurement matrix H_k
1557
1/2
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
 25517 _kalmanFilter.H = n_measurementMatrix_H(T_rn_p, n_Dcm_b, b_omega_ip, b_leverArm_InsGnss, n_Omega_ie);
1558
1559 // 6. Calculate the measurement noise covariance matrix R_k
1560
1/2
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
 25517 _kalmanFilter.R = n_measurementNoiseCovariance_R(posVelObs, lla_position, R_N, R_E);
1561
1562 // 8. Formulate the measurement z_k
1563
3/6
✓ Branch 2 taken 25517 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 25517 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 25517 times.
✗ Branch 12 not taken.
 51034 _kalmanFilter.z = n_measurementInnovation_dz(posVelObs->lla_position(), _inertialIntegrator.getLatestState().value().get().position,
1564
1/2
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
 25517 posVelObs->n_velocity(), _inertialIntegrator.getLatestState().value().get().velocity,
1565
3/6
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 25517 times.
✗ Branch 9 not taken.
 76551 T_rn_p, _inertialIntegrator.getLatestState().value().get().attitude, b_leverArm_InsGnss, b_omega_ip, n_Omega_ie);
1566 }
1567 else // if (_inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::ECEF)
1568 {
1569 // Direction Cosine Matrix from body to navigation coordinates, at the time tₖ₋₁
1570
3/6
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1296 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1296 times.
✗ Branch 9 not taken.
 1296 Eigen::Matrix3d e_Dcm_b = _inertialIntegrator.getLatestState().value().get().attitude.toRotationMatrix();
1571 LOG_DATA("{}: e_Dcm_b =\n{}", nameId(), e_Dcm_b);
1572
1573 // Skew-symmetric matrix of the Earth-rotation vector in local navigation frame axes
1574
1/2
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
 1296 Eigen::Matrix3d e_Omega_ie = math::skewSymmetricMatrix(InsConst::e_omega_ie);
1575 LOG_DATA("{}: e_Omega_ie =\n{}", nameId(), e_Omega_ie);
1576
1577 // 5. Calculate the measurement matrix H_k
1578
1/2
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
 1296 _kalmanFilter.H = e_measurementMatrix_H(e_Dcm_b, b_omega_ip, b_leverArm_InsGnss, e_Omega_ie);
1579
1580 // 6. Calculate the measurement noise covariance matrix R_k
1581
1/2
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
 1296 _kalmanFilter.R = e_measurementNoiseCovariance_R(posVelObs);
1582
1583 // 8. Formulate the measurement z_k
1584
3/6
✓ Branch 2 taken 1296 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1296 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1296 times.
✗ Branch 12 not taken.
 2592 _kalmanFilter.z = e_measurementInnovation_dz(posVelObs->e_position(), _inertialIntegrator.getLatestState().value().get().position,
1585
1/2
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
 1296 posVelObs->e_velocity(), _inertialIntegrator.getLatestState().value().get().velocity,
1586
3/6
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1296 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1296 times.
✗ Branch 9 not taken.
 3888 _inertialIntegrator.getLatestState().value().get().attitude, b_leverArm_InsGnss, b_omega_ip, e_Omega_ie);
1587 }
1588
1589 LOG_DATA("{}: KF.H =\n{}", nameId(), _kalmanFilter.H);
1590 LOG_DATA("{}: KF.R =\n{}", nameId(), _kalmanFilter.R);
1591 LOG_DATA("{}: KF.z =\n{}", nameId(), _kalmanFilter.z);
1592
1593
1/2
✓ Branch 0 taken 26813 times.
✗ Branch 1 not taken.
 26813 if (_checkKalmanMatricesRanks)
1594 {
1595
5/10
✓ Branch 3 taken 26813 times.
✗ Branch 4 not taken.
✓ Branch 8 taken 26813 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 26813 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 26813 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 26813 times.
✗ Branch 18 not taken.
 26813 Eigen::FullPivLU<Eigen::MatrixXd> lu(_kalmanFilter.H(all, all) * _kalmanFilter.P(all, all) * _kalmanFilter.H(all, all).transpose() + _kalmanFilter.R(all, all));
1596
1/2
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
 26813 auto rank = lu.rank();
1597
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 26813 times.
 26813 if (rank != _kalmanFilter.H(all, all).rows())
1598 {
1599 ✗ LOG_WARN("{}: [{}] (HPH^T + R).rank = {}", nameId(), posVelObs->insTime.toYMDHMS(GPST), rank);
1600 }
1601 26813 }
1602
1603 // 7. Calculate the Kalman gain matrix K_k
1604 // 9. Update the state vector estimate from x(-) to x(+)
1605 // 10. Update the error covariance matrix from P(-) to P(+)
1606
1/2
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
 26813 _kalmanFilter.correctWithMeasurementInnovation();
1607
1608 LOG_DATA("{}: KF.K =\n{}", nameId(), _kalmanFilter.K);
1609 LOG_DATA("{}: KF.x =\n{}", nameId(), _kalmanFilter.x);
1610 LOG_DATA("{}: KF.P =\n{}", nameId(), _kalmanFilter.P);
1611
1612 // Averaging of P to avoid numerical problems with symmetry (did not work)
1613 // _kalmanFilter.P = ((_kalmanFilter.P + _kalmanFilter.P.transpose()) / 2.0);
1614
1615
1/2
✓ Branch 0 taken 26813 times.
✗ Branch 1 not taken.
 26813 if (_checkKalmanMatricesRanks)
1616 {
1617
5/10
✓ Branch 3 taken 26813 times.
✗ Branch 4 not taken.
✓ Branch 8 taken 26813 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 26813 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 26813 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 26813 times.
✗ Branch 18 not taken.
 26813 Eigen::FullPivLU<Eigen::MatrixXd> lu(_kalmanFilter.H(all, all) * _kalmanFilter.P(all, all) * _kalmanFilter.H(all, all).transpose() + _kalmanFilter.R(all, all));
1618
1/2
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
 26813 auto rank = lu.rank();
1619
2/2
✓ Branch 2 taken 2592 times.
✓ Branch 3 taken 24221 times.
 26813 if (rank != _kalmanFilter.H(all, all).rows())
1620 {
1621
4/8
✓ Branch 1 taken 2592 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 2592 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2592 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 2592 times.
✗ Branch 14 not taken.
 2592 LOG_WARN("{}: [{}] (HPH^T + R).rank = {}", nameId(), posVelObs->insTime.toYMDHMS(GPST), rank);
1622 }
1623
1624
1/2
✓ Branch 2 taken 26813 times.
✗ Branch 3 not taken.
 26813 Eigen::FullPivLU<Eigen::MatrixXd> luP(_kalmanFilter.P(all, all));
1625
1/2
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
 26813 rank = luP.rank();
1626
2/2
✓ Branch 2 taken 2592 times.
✓ Branch 3 taken 24221 times.
 26813 if (rank != _kalmanFilter.P(all, all).rows())
1627 {
1628
4/8
✓ Branch 1 taken 2592 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 2592 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2592 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 2592 times.
✗ Branch 14 not taken.
 2592 LOG_WARN("{}: [{}] P.rank = {}", nameId(), posVelObs->insTime.toYMDHMS(GPST), rank);
1629 }
1630 26813 }
1631
1632 // LOG_DEBUG("{}: H\n{}\n", nameId(), _kalmanFilter.H);
1633 // LOG_DEBUG("{}: R\n{}\n", nameId(), _kalmanFilter.R);
1634 // LOG_DEBUG("{}: z =\n{}", nameId(), _kalmanFilter.z.transposed());
1635
1636 // LOG_DEBUG("{}: K\n{}\n", nameId(), _kalmanFilter.K);
1637 // LOG_DEBUG("{}: x =\n{}", nameId(), _kalmanFilter.x.transposed());
1638 // LOG_DEBUG("{}: P\n{}\n", nameId(), _kalmanFilter.P);
1639
1640 // LOG_DEBUG("{}: K * z =\n{}", nameId(), (_kalmanFilter.K(all, all) * _kalmanFilter.z(all)).transpose());
1641
1642 // LOG_DEBUG("{}: P - P^T\n{}\n", nameId(), _kalmanFilter.P(all, all) - _kalmanFilter.P(all, all).transpose());
1643
1644 // Push out the new data
1645
1/2
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
 26813 auto lckfSolution = std::make_shared<InsGnssLCKFSolution>();
1646 26813 lckfSolution->insTime = posVelObs->insTime;
1647
2/4
✓ Branch 2 taken 26813 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 26813 times.
✗ Branch 7 not taken.
 26813 lckfSolution->positionError = _kalmanFilter.x.segment<3>(KFPos);
1648
2/4
✓ Branch 2 taken 26813 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 26813 times.
✗ Branch 7 not taken.
 26813 lckfSolution->velocityError = _kalmanFilter.x.segment<3>(KFVel);
1649
3/6
✓ Branch 2 taken 26813 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 26813 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 26813 times.
✗ Branch 10 not taken.
 26813 lckfSolution->attitudeError = _kalmanFilter.x.segment<3>(KFAtt) * (1. / SCALE_FACTOR_ATTITUDE);
1650
1651 LOG_DATA("{}: Accumulated biases before error has been applied: b_biasAccel.value = {}, b_biasGyro.value = {}", nameId(), _inertialIntegrator.p_getLastAccelerationBias().transpose(), _inertialIntegrator.p_getLastAngularRateBias().transpose());
1652
1653
8/16
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 26813 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 26813 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 26813 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 26813 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 26813 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 26813 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 26813 times.
✗ Branch 25 not taken.
 26813 _inertialIntegrator.applySensorBiasesIncrements(_lastImuObs->imuPos.p_quat_b() * -_kalmanFilter.x.segment<3>(KFAccBias) * (1. / SCALE_FACTOR_ACCELERATION),
1654
5/10
✓ Branch 2 taken 26813 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 26813 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 26813 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 26813 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 26813 times.
✗ Branch 16 not taken.
 26813 _lastImuObs->imuPos.p_quat_b() * -_kalmanFilter.x.segment<3>(KFGyrBias) * (1. / SCALE_FACTOR_ANGULAR_RATE));
1655
3/6
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26813 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 26813 times.
✗ Branch 9 not taken.
 26813 lckfSolution->b_biasAccel.value = -_inertialIntegrator.p_getLastAccelerationBias();
1656 26813 lckfSolution->b_biasAccel.stdDev = Eigen::Vector3d{
1657
1/2
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
 26813 std::sqrt(_kalmanFilter.P(KFStates::AccBiasX, KFStates::AccBiasX) * (1. / std::pow(SCALE_FACTOR_ACCELERATION, 2))),
1658
1/2
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
 26813 std::sqrt(_kalmanFilter.P(KFStates::AccBiasY, KFStates::AccBiasY) * (1. / std::pow(SCALE_FACTOR_ACCELERATION, 2))),
1659
2/4
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 26813 times.
✗ Branch 6 not taken.
 26813 std::sqrt(_kalmanFilter.P(KFStates::AccBiasZ, KFStates::AccBiasZ) * (1. / std::pow(SCALE_FACTOR_ACCELERATION, 2)))
1660 26813 };
1661
3/6
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26813 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 26813 times.
✗ Branch 9 not taken.
 26813 lckfSolution->b_biasGyro.value = -_inertialIntegrator.p_getLastAngularRateBias();
1662 26813 lckfSolution->b_biasGyro.stdDev = Eigen::Vector3d{
1663
1/2
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
 26813 std::sqrt(_kalmanFilter.P(KFStates::GyrBiasX, KFStates::GyrBiasX) * (1. / std::pow(SCALE_FACTOR_ANGULAR_RATE, 2))),
1664
1/2
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
 26813 std::sqrt(_kalmanFilter.P(KFStates::GyrBiasY, KFStates::GyrBiasY) * (1. / std::pow(SCALE_FACTOR_ANGULAR_RATE, 2))),
1665
2/4
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 26813 times.
✗ Branch 6 not taken.
 26813 std::sqrt(_kalmanFilter.P(KFStates::GyrBiasZ, KFStates::GyrBiasZ) * (1. / std::pow(SCALE_FACTOR_ANGULAR_RATE, 2)))
1666 26813 };
1667
1668 LOG_DATA("{}: Biases after error has been applied: b_biasAccel.value = {}, b_biasGyro.value = {}", nameId(), lckfSolution->b_biasAccel.value.transpose(), lckfSolution->b_biasGyro.value.transpose());
1669
1670
3/4
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 25517 times.
✓ Branch 4 taken 1296 times.
 26813 if (_inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::NED)
1671 {
1672
2/4
✓ Branch 2 taken 25517 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 25517 times.
✗ Branch 6 not taken.
 25517 lckfSolution->positionError.topRows<2>() *= 1.0 / SCALE_FACTOR_LAT_LON;
1673 25517 lckfSolution->frame = InsGnssLCKFSolution::Frame::NED;
1674
1/2
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
 25517 _inertialIntegrator.applyStateErrors_n(lckfSolution->positionError,
1675 25517 lckfSolution->velocityError,
1676 25517 lckfSolution->attitudeError);
1677 }
1678 else // if (_inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::ECEF)
1679 {
1680 1296 lckfSolution->frame = InsGnssLCKFSolution::Frame::ECEF;
1681
1/2
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
 1296 _inertialIntegrator.applyStateErrors_e(lckfSolution->positionError,
1682 1296 lckfSolution->velocityError,
1683 1296 lckfSolution->attitudeError);
1684 }
1685
1686
1/2
✓ Branch 2 taken 26813 times.
✗ Branch 3 not taken.
 26813 setSolutionPosVelAttAndCov(lckfSolution, R_N, R_E);
1687
1688
1/2
✓ Branch 2 taken 26813 times.
✗ Branch 3 not taken.
 26813 lckfSolution->heightBias = { .value = _heightBiasTotal, .stdDev = std::sqrt(_kalmanFilter.P(KFStates::HeightBias, KFStates::HeightBias)) };
1689
1/2
✓ Branch 2 taken 26813 times.
✗ Branch 3 not taken.
 26813 lckfSolution->heightScale = { .value = _heightScaleTotal, .stdDev = std::sqrt(_kalmanFilter.P(KFStates::HeightScale, KFStates::HeightScale)) };
1690
1691 // Closed loop
1692
1/2
✓ Branch 2 taken 26813 times.
✗ Branch 3 not taken.
 26813 _kalmanFilter.x(all).setZero();
1693
1694 LOG_DATA("{}: [{}] Sending out updated solution", nameId(), lckfSolution->insTime.toYMDHMS(GPST));
1695
2/4
✓ Branch 2 taken 26813 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 26813 times.
✗ Branch 5 not taken.
 26813 if (!hasInputPinWithSameTime(lckfSolution->insTime))
1696 {
1697
1/2
✓ Branch 2 taken 26813 times.
✗ Branch 3 not taken.
 26813 invokeCallbacks(OUTPUT_PORT_INDEX_SOLUTION, lckfSolution);
1698 }
1699 26813 }
1700
1701 ✗ void NAV::LooselyCoupledKF::looselyCoupledUpdate(const std::shared_ptr<const BaroHgt>& baroHgtObs)
1702 {
1703 ✗ INS_ASSERT_USER_ERROR(_inertialIntegrator.getLatestState().has_value(), "The update should not even trigger without an initial state.");
1704
1705 LOG_DATA("{}: [{}] Updating (lastInertial at [{}])", nameId(), baroHgtObs->insTime.toYMDHMS(GPST), _inertialIntegrator.getLatestState().value().get().epoch.toYMDHMS(GPST));
1706
1707 // -------------------------------------------- GUI Parameters -----------------------------------------------
1708
1709 // Latitude 𝜙, longitude λ and altitude (height above ground) in [rad, rad, m] at the time tₖ₋₁
1710 ✗ Eigen::Vector3d lla_position;
1711 ✗ if (_inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::NED)
1712 {
1713 ✗ lla_position = _inertialIntegrator.getLatestState().value().get().position;
1714 }
1715 else // if (_inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::ECEF)
1716 {
1717 ✗ lla_position = trafo::lla2ecef_WGS84(_inertialIntegrator.getLatestState().value().get().position);
1718 }
1719 LOG_DATA("{}: lla_position = {} [rad, rad, m]", nameId(), lla_position.transpose());
1720
1721 // Baro height measurement uncertainty (Variance σ²) in [m^2]
1722 ✗ double baroHeightSigmaSquared{};
1723 ✗ if (_baroHeightMeasurementUncertaintyOverride || !baroHgtObs->baro_heightStdev.has_value())
1724 {
1725 ✗ switch (_barometricHeightMeasurementUncertaintyUnit)
1726 {
1727 ✗ case BaroHeightMeasurementUncertaintyUnit::m:
1728 ✗ baroHeightSigmaSquared = std::pow(_barometricHeightMeasurementUncertainty, 2);
1729 ✗ break;
1730 ✗ case BaroHeightMeasurementUncertaintyUnit::m2:
1731 ✗ baroHeightSigmaSquared = _barometricHeightMeasurementUncertainty;
1732 ✗ break;
1733 }
1734 }
1735 else
1736 {
1737 ✗ baroHeightSigmaSquared = std::pow(baroHgtObs->baro_heightStdev.value(), 2);
1738 }
1739 LOG_DATA("{}: baroHeightSigmaSquared = {} [m^2]", nameId(), baroHeightSigmaSquared);
1740
1741 // ---------------------------------------------- Correction -------------------------------------------------
1742 ✗ _kalmanFilter.setMeasurements(std::array{ KFMeas::dHgt });
1743
1744 // 5. Calculate the measurement matrix H_k
1745 ✗ if (_inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::NED)
1746 {
1747 ✗ _kalmanFilter.H = n_measurementMatrix_H(lla_position(2), _heightScaleTotal);
1748 }
1749 else // if (_inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::ECEF)
1750 {
1751 ✗ _kalmanFilter.H = e_measurementMatrix_H(_inertialIntegrator.getLatestState().value().get().position, lla_position(2), _heightScaleTotal);
1752 }
1753
1754 // 6. Calculate the measurement noise covariance matrix R_k (here we can use the n-Frame covariance matrix! )
1755 ✗ _kalmanFilter.R = n_measurementNoiseCovariance_R(baroHeightSigmaSquared);
1756
1757 // 8. Formulate the measurement z_k
1758 ✗ _kalmanFilter.z = n_measurementInnovation_dz(baroHgtObs->baro_height, lla_position(2), _heightBiasTotal, _heightScaleTotal);
1759
1760 LOG_DATA("{}: KF.H =\n{}", nameId(), _kalmanFilter.H);
1761 LOG_DATA("{}: KF.R =\n{}", nameId(), _kalmanFilter.R);
1762 LOG_DATA("{}: KF.z =\n{}", nameId(), _kalmanFilter.z);
1763
1764 ✗ if (_checkKalmanMatricesRanks)
1765 {
1766 ✗ Eigen::FullPivLU<Eigen::MatrixXd> lu(_kalmanFilter.H(all, all) * _kalmanFilter.P(all, all) * _kalmanFilter.H(all, all).transpose() + _kalmanFilter.R(all, all));
1767 ✗ auto rank = lu.rank();
1768 ✗ if (rank != _kalmanFilter.H(all, all).rows())
1769 {
1770 ✗ LOG_WARN("{}: [{}] (HPH^T + R).rank = {}", nameId(), baroHgtObs->insTime.toYMDHMS(GPST), rank);
1771 }
1772 ✗ }
1773
1774 // 7. Calculate the Kalman gain matrix K_k
1775 // 9. Update the state vector estimate from x(-) to x(+)
1776 // 10. Update the error covariance matrix from P(-) to P(+
1777 ✗ _kalmanFilter.correctWithMeasurementInnovation();
1778
1779 LOG_DATA("{}: KF.K =\n{}", nameId(), _kalmanFilter.K);
1780 LOG_DATA("{}: KF.x =\n{}", nameId(), _kalmanFilter.x);
1781 LOG_DATA("{}: KF.P =\n{}", nameId(), _kalmanFilter.P);
1782
1783 // Averaging of P to avoid numerical problems with symmetry (did not work)
1784 // _kalmanFilter.P = ((_kalmanFilter.P + _kalmanFilter.P.transpose()) / 2.0);
1785
1786 ✗ if (_checkKalmanMatricesRanks)
1787 {
1788 ✗ Eigen::FullPivLU<Eigen::MatrixXd> lu(_kalmanFilter.H(all, all) * _kalmanFilter.P(all, all) * _kalmanFilter.H(all, all).transpose() + _kalmanFilter.R(all, all));
1789 ✗ auto rank = lu.rank();
1790 ✗ if (rank != _kalmanFilter.H(all, all).rows())
1791 {
1792 ✗ LOG_WARN("{}: [{}] (HPH^T + R).rank = {}", nameId(), baroHgtObs->insTime.toYMDHMS(GPST), rank);
1793 }
1794
1795 ✗ Eigen::FullPivLU<Eigen::MatrixXd> luP(_kalmanFilter.P(all, all));
1796 ✗ rank = luP.rank();
1797 ✗ if (rank != _kalmanFilter.P(all, all).rows())
1798 {
1799 ✗ LOG_WARN("{}: [{}] P.rank = {}", nameId(), baroHgtObs->insTime.toYMDHMS(GPST), rank);
1800 }
1801 ✗ }
1802
1803 // LOG_DEBUG("{}: H\n{}\n", nameId(), _kalmanFilter.H);
1804 // LOG_DEBUG("{}: R\n{}\n", nameId(), _kalmanFilter.R);
1805 // LOG_DEBUG("{}: z =\n{}", nameId(), _kalmanFilter.z.transposed());
1806
1807 // LOG_DEBUG("{}: K\n{}\n", nameId(), _kalmanFilter.K);
1808 // LOG_DEBUG("{}: x =\n{}", nameId(), _kalmanFilter.x.transposed());
1809 // LOG_DEBUG("{}: P\n{}\n", nameId(), _kalmanFilter.P);
1810
1811 // LOG_DEBUG("{}: K * z =\n{}", nameId(), (_kalmanFilter.K(all, all) * _kalmanFilter.z(all)).transpose());
1812
1813 // LOG_DEBUG("{}: P - P^T\n{}\n", nameId(), _kalmanFilter.P(all, all) - _kalmanFilter.P(all, all).transpose());
1814
1815 // Push out the new data
1816 ✗ auto lckfSolution = std::make_shared<InsGnssLCKFSolution>();
1817 ✗ lckfSolution->insTime = baroHgtObs->insTime;
1818 ✗ lckfSolution->positionError = _kalmanFilter.x.segment<3>(KFPos);
1819 ✗ lckfSolution->velocityError = _kalmanFilter.x.segment<3>(KFVel);
1820 ✗ lckfSolution->attitudeError = _kalmanFilter.x.segment<3>(KFAtt) * (1. / SCALE_FACTOR_ATTITUDE);
1821
1822 LOG_DATA("{}: Accumulated biases before error has been applied: b_biasAccel.value = {}, b_biasGyro.value = {}", nameId(), _inertialIntegrator.p_getLastAccelerationBias().transpose(), _inertialIntegrator.p_getLastAngularRateBias().transpose());
1823
1824 ✗ _inertialIntegrator.applySensorBiasesIncrements(_lastImuObs->imuPos.p_quat_b() * -_kalmanFilter.x.segment<3>(KFAccBias) * (1. / SCALE_FACTOR_ACCELERATION),
1825 ✗ _lastImuObs->imuPos.p_quat_b() * -_kalmanFilter.x.segment<3>(KFGyrBias) * (1. / SCALE_FACTOR_ANGULAR_RATE));
1826 ✗ lckfSolution->b_biasAccel.value = -_inertialIntegrator.p_getLastAccelerationBias();
1827 ✗ lckfSolution->b_biasAccel.stdDev = Eigen::Vector3d{
1828 ✗ std::sqrt(_kalmanFilter.P(KFStates::AccBiasX, KFStates::AccBiasX) * (1. / std::pow(SCALE_FACTOR_ACCELERATION, 2))),
1829 ✗ std::sqrt(_kalmanFilter.P(KFStates::AccBiasY, KFStates::AccBiasY) * (1. / std::pow(SCALE_FACTOR_ACCELERATION, 2))),
1830 ✗ std::sqrt(_kalmanFilter.P(KFStates::AccBiasZ, KFStates::AccBiasZ) * (1. / std::pow(SCALE_FACTOR_ACCELERATION, 2)))
1831 ✗ };
1832 ✗ lckfSolution->b_biasGyro.value = -_inertialIntegrator.p_getLastAngularRateBias();
1833 ✗ lckfSolution->b_biasGyro.stdDev = Eigen::Vector3d{
1834 ✗ std::sqrt(_kalmanFilter.P(KFStates::GyrBiasX, KFStates::GyrBiasX) * (1. / std::pow(SCALE_FACTOR_ANGULAR_RATE, 2))),
1835 ✗ std::sqrt(_kalmanFilter.P(KFStates::GyrBiasY, KFStates::GyrBiasY) * (1. / std::pow(SCALE_FACTOR_ANGULAR_RATE, 2))),
1836 ✗ std::sqrt(_kalmanFilter.P(KFStates::GyrBiasZ, KFStates::GyrBiasZ) * (1. / std::pow(SCALE_FACTOR_ANGULAR_RATE, 2)))
1837 ✗ };
1838
1839 LOG_DATA("{}: Biases after error has been applied: b_biasAccel.value = {}, b_biasGyro.value = {}", nameId(), lckfSolution->b_biasAccel.value.transpose(), lckfSolution->b_biasGyro.value.transpose());
1840
1841 ✗ if (_inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::NED)
1842 {
1843 ✗ lckfSolution->positionError.topRows<2>() *= 1.0 / SCALE_FACTOR_LAT_LON;
1844 ✗ lckfSolution->frame = InsGnssLCKFSolution::Frame::NED;
1845 ✗ _inertialIntegrator.applyStateErrors_n(lckfSolution->positionError,
1846 ✗ lckfSolution->velocityError,
1847 ✗ lckfSolution->attitudeError);
1848 }
1849 else // if (_inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::ECEF)
1850 {
1851 ✗ lckfSolution->frame = InsGnssLCKFSolution::Frame::ECEF;
1852 ✗ _inertialIntegrator.applyStateErrors_e(lckfSolution->positionError,
1853 ✗ lckfSolution->velocityError,
1854 ✗ lckfSolution->attitudeError);
1855 }
1856
1857 ✗ setSolutionPosVelAttAndCov(lckfSolution, calcEarthRadius_N(lla_position.x()), calcEarthRadius_E(lla_position.x()));
1858
1859 // Closed loop
1860 ✗ _heightBiasTotal += _kalmanFilter.x(KFStates::HeightBias);
1861 ✗ _heightScaleTotal += _kalmanFilter.x(KFStates::HeightScale);
1862 ✗ lckfSolution->heightBias = { .value = _heightBiasTotal, .stdDev = std::sqrt(_kalmanFilter.P(KFStates::HeightBias, KFStates::HeightBias)) };
1863 ✗ lckfSolution->heightScale = { .value = _heightScaleTotal, .stdDev = std::sqrt(_kalmanFilter.P(KFStates::HeightScale, KFStates::HeightScale)) };
1864 ✗ _kalmanFilter.x(all).setZero();
1865
1866 LOG_DATA("{}: [{}] Sending out updated solution", nameId(), lckfSolution->insTime.toYMDHMS(GPST));
1867 ✗ if (!hasInputPinWithSameTime(lckfSolution->insTime))
1868 {
1869 ✗ invokeCallbacks(OUTPUT_PORT_INDEX_SOLUTION, lckfSolution);
1870 }
1871 ✗ }
1872
1873 76810 void NAV::LooselyCoupledKF::setSolutionPosVelAttAndCov(const std::shared_ptr<PosVelAtt>& lckfSolution, double R_N, double R_E)
1874 {
1875
2/4
✓ Branch 1 taken 76810 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76810 times.
✗ Branch 5 not taken.
 76810 Eigen::Matrix<double, 10, 9> J = Eigen::Matrix<double, 10, 9>::Zero();
1876
2/4
✓ Branch 1 taken 76810 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76810 times.
✗ Branch 5 not taken.
 76810 J.topLeftCorner<6, 6>().setIdentity();
1877
1878
2/4
✓ Branch 1 taken 76810 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76810 times.
✗ Branch 5 not taken.
 76810 Eigen::Matrix9d J_units = Eigen::Matrix9d::Identity();
1879
3/6
✓ Branch 1 taken 76810 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76810 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 76810 times.
✗ Branch 8 not taken.
 76810 J_units.bottomRightCorner<3, 3>().diagonal().setConstant(std::numbers::pi_v<double> / 180.0);
1880
3/4
✓ Branch 1 taken 76810 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 62626 times.
✓ Branch 4 taken 14184 times.
 76810 if (_inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::NED)
1881 {
1882
2/4
✓ Branch 1 taken 62626 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 62626 times.
✗ Branch 5 not taken.
 62626 const Eigen::Vector3d& lla_position = _inertialIntegrator.getLatestState().value().get().position;
1883
1884
5/10
✓ Branch 1 taken 62626 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 62626 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 62626 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 62626 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 62626 times.
✗ Branch 15 not taken.
 62626 J.bottomRightCorner<4, 3>() = trafo::covRPY2quatJacobian(_inertialIntegrator.getLatestState().value().get().attitude);
1885
2/4
✓ Branch 1 taken 62626 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 62626 times.
✗ Branch 5 not taken.
 62626 J_units.topLeftCorner<3, 3>().diagonal() = 1.0
1886
2/4
✓ Branch 1 taken 62626 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 62626 times.
✗ Branch 5 not taken.
 125252 / n_F_dr_dv(lla_position.x(),
1887
2/4
✓ Branch 1 taken 62626 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 62626 times.
✗ Branch 5 not taken.
 62626 lla_position.z(),
1888 R_N,
1889 R_E)
1890
1/2
✓ Branch 1 taken 62626 times.
✗ Branch 2 not taken.
 62626 .diagonal()
1891
2/4
✓ Branch 1 taken 62626 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 62626 times.
✗ Branch 5 not taken.
 125252 .array();
1892
1893
3/6
✓ Branch 1 taken 62626 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 62626 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 62626 times.
✗ Branch 8 not taken.
 62626 J_units.topLeftCorner<2, 2>().diagonal() *= 1.0 / SCALE_FACTOR_LAT_LON;
1894
1895 62626 lckfSolution->setPosVelAttAndCov_n(lla_position,
1896
2/4
✓ Branch 1 taken 62626 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 62626 times.
✗ Branch 6 not taken.
 62626 _inertialIntegrator.getLatestState().value().get().velocity,
1897
3/6
✓ Branch 1 taken 62626 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 62626 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 62626 times.
✗ Branch 9 not taken.
 62626 _inertialIntegrator.getLatestState().value().get().attitude,
1898
7/14
✓ Branch 1 taken 62626 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 62626 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 62626 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 62626 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 62626 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 62626 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 62626 times.
✗ Branch 22 not taken.
 125252 J * (J_units * _kalmanFilter.P(KFPosVelAtt, KFPosVelAtt) * J_units.transpose()) * J.transpose());
1899 }
1900 else // if (_inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::ECEF)
1901 {
1902
5/10
✓ Branch 1 taken 14184 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14184 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 14184 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 14184 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 14184 times.
✗ Branch 15 not taken.
 14184 J.bottomRightCorner<4, 3>() = trafo::covRPY2quatJacobian(_inertialIntegrator.getLatestState().value().get().attitude);
1903
1904
2/4
✓ Branch 2 taken 14184 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 14184 times.
✗ Branch 7 not taken.
 28368 lckfSolution->setPosVelAttAndCov_e(_inertialIntegrator.getLatestState().value().get().position,
1905
2/4
✓ Branch 1 taken 14184 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 14184 times.
✗ Branch 6 not taken.
 14184 _inertialIntegrator.getLatestState().value().get().velocity,
1906
3/6
✓ Branch 1 taken 14184 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14184 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 14184 times.
✗ Branch 9 not taken.
 14184 _inertialIntegrator.getLatestState().value().get().attitude,
1907
7/14
✓ Branch 1 taken 14184 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14184 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 14184 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 14184 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 14184 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 14184 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 14184 times.
✗ Branch 22 not taken.
 28368 J * (J_units * _kalmanFilter.P(KFPosVelAtt, KFPosVelAtt) * J_units.transpose()) * J.transpose());
1908 }
1909 76810 }
1910
1911 // ###########################################################################################################
1912 // System matrix 𝐅
1913 // ###########################################################################################################
1914
1915 NAV::KeyedMatrix<double, NAV::LooselyCoupledKF::KFStates, NAV::LooselyCoupledKF::KFStates, NAV::LooselyCoupledKF::KFStates_COUNT, NAV::LooselyCoupledKF::KFStates_COUNT>
1916 37109 NAV::LooselyCoupledKF::n_systemMatrix_F(const Eigen::Quaterniond& n_Quat_b,
1917 const Eigen::Vector3d& b_specForce_ib,
1918 const Eigen::Vector3d& n_omega_in,
1919 const Eigen::Vector3d& n_velocity,
1920 const Eigen::Vector3d& lla_position,
1921 double R_N,
1922 double R_E,
1923 double g_0,
1924 double r_eS_e,
1925 const Eigen::Vector3d& tau_bad,
1926 const Eigen::Vector3d& tau_bgd) const
1927 {
1928
1/2
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
 37109 double latitude = lla_position(0); // Geodetic latitude of the body in [rad]
1929
1/2
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
 37109 double altitude = lla_position(2); // Geodetic height of the body in [m]
1930
1931
3/6
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 37109 times.
✗ Branch 8 not taken.
 37109 Eigen::Vector3d beta_bad = 1. / tau_bad.array(); // Gauss-Markov constant for the accelerometer 𝛽 = 1 / 𝜏 (𝜏 correlation length)
1932
3/6
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 37109 times.
✗ Branch 8 not taken.
 37109 Eigen::Vector3d beta_bgd = 1. / tau_bgd.array(); // Gauss-Markov constant for the gyroscope 𝛽 = 1 / 𝜏 (𝜏 correlation length)
1933
1934 // System matrix 𝐅
1935 // Math: \mathbf{F}^n = \begin{pmatrix} \mathbf{F}_{\dot{\psi},\psi}^n & \mathbf{F}_{\dot{\psi},\delta v}^n & \mathbf{F}_{\dot{\psi},\delta r}^n & \mathbf{0}_3 & \mathbf{C}_b^n \\ \mathbf{F}_{\delta \dot{v},\psi}^n & \mathbf{F}_{\delta \dot{v},\delta v}^n & \mathbf{F}_{\delta \dot{v},\delta r}^n & \mathbf{C}_b^n & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{F}_{\delta \dot{r},\delta v}^n & \mathbf{F}_{\delta \dot{r},\delta r}^n & \mathbf{0}_3 & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \vee -\mathbf{\beta} & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \vee -\mathbf{\beta} \end{pmatrix}
1936 KeyedMatrix<double, KFStates, KFStates, NAV::LooselyCoupledKF::KFStates_COUNT, NAV::LooselyCoupledKF::KFStates_COUNT>
1937
2/4
✓ Branch 2 taken 37109 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 37109 times.
✗ Branch 6 not taken.
 37109 F(Eigen::Matrix<double, NAV::LooselyCoupledKF::KFStates_COUNT, NAV::LooselyCoupledKF::KFStates_COUNT>::Zero(), States);
1938
1939
3/6
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 37109 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 37109 times.
✗ Branch 10 not taken.
 37109 F.block<3>(KFAtt, KFAtt) = n_F_dpsi_dpsi(n_omega_in);
1940
3/6
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 37109 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 37109 times.
✗ Branch 10 not taken.
 37109 F.block<3>(KFAtt, KFVel) = n_F_dpsi_dv(latitude, altitude, R_N, R_E);
1941
3/6
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 37109 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 37109 times.
✗ Branch 10 not taken.
 37109 F.block<3>(KFAtt, KFPos) = n_F_dpsi_dr(latitude, altitude, n_velocity, R_N, R_E);
1942
4/8
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 37109 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 37109 times.
✗ Branch 13 not taken.
 37109 F.block<3>(KFAtt, KFGyrBias) = n_F_dpsi_dw(n_Quat_b.toRotationMatrix());
1943
4/8
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 37109 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 37109 times.
✗ Branch 13 not taken.
 37109 F.block<3>(KFVel, KFAtt) = n_F_dv_dpsi(n_Quat_b * b_specForce_ib);
1944
3/6
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 37109 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 37109 times.
✗ Branch 10 not taken.
 37109 F.block<3>(KFVel, KFVel) = n_F_dv_dv(n_velocity, latitude, altitude, R_N, R_E);
1945
3/6
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 37109 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 37109 times.
✗ Branch 10 not taken.
 37109 F.block<3>(KFVel, KFPos) = n_F_dv_dr(n_velocity, latitude, altitude, R_N, R_E, g_0, r_eS_e);
1946
4/8
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 37109 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 37109 times.
✗ Branch 13 not taken.
 37109 F.block<3>(KFVel, KFAccBias) = n_F_dv_df(n_Quat_b.toRotationMatrix());
1947
3/6
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 37109 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 37109 times.
✗ Branch 10 not taken.
 37109 F.block<3>(KFPos, KFVel) = n_F_dr_dv(latitude, altitude, R_N, R_E);
1948
3/6
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 37109 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 37109 times.
✗ Branch 10 not taken.
 37109 F.block<3>(KFPos, KFPos) = n_F_dr_dr(n_velocity, latitude, altitude, R_N, R_E);
1949
2/2
✓ Branch 0 taken 30665 times.
✓ Branch 1 taken 6444 times.
 37109 if (_qCalculationAlgorithm == QCalculationAlgorithm::VanLoan)
1950 {
1951
8/14
✓ Branch 0 taken 24221 times.
✓ Branch 1 taken 6444 times.
✓ Branch 3 taken 24221 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 24221 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6444 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 30665 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 30665 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 30665 times.
✗ Branch 21 not taken.
 30665 F.block<3>(KFAccBias, KFAccBias) = n_F_df_df(_randomProcessAccel == RandomProcess::RandomWalk ? Eigen::Vector3d::Zero() : beta_bad);
1952
8/14
✓ Branch 0 taken 24221 times.
✓ Branch 1 taken 6444 times.
✓ Branch 3 taken 24221 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 24221 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6444 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 30665 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 30665 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 30665 times.
✗ Branch 21 not taken.
 30665 F.block<3>(KFGyrBias, KFGyrBias) = n_F_dw_dw(_randomProcessGyro == RandomProcess::RandomWalk ? Eigen::Vector3d::Zero() : beta_bgd);
1953 }
1954
1955
2/4
✓ Branch 2 taken 37109 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 37109 times.
✗ Branch 6 not taken.
 37109 F.middleRows<3>(KFAtt) *= SCALE_FACTOR_ATTITUDE; // 𝜓' [deg / s] = 180/π * ... [rad / s]
1956
2/4
✓ Branch 2 taken 37109 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 37109 times.
✗ Branch 6 not taken.
 37109 F.middleCols<3>(KFAtt) *= 1. / SCALE_FACTOR_ATTITUDE;
1957
1958 // F.middleRows<3>(Vel) *= 1.; // 𝛿v' [m / s^2] = 1 * [m / s^2]
1959 // F.middleCols<3>(Vel) *= 1. / 1.;
1960
1961
2/4
✓ Branch 2 taken 37109 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 37109 times.
✗ Branch 6 not taken.
 37109 F.middleRows<2>(std::array{ PosLat, PosLon }) *= SCALE_FACTOR_LAT_LON; // 𝛿ϕ' [pseudometre / s] = R0 * [rad / s]
1962
2/4
✓ Branch 2 taken 37109 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 37109 times.
✗ Branch 6 not taken.
 37109 F.middleCols<2>(std::array{ PosLat, PosLon }) *= 1. / SCALE_FACTOR_LAT_LON;
1963 // F.row(PosAlt) *= 1.; // 𝛿h' [m / s] = 1 * [m / s]
1964 // F.col(PosAlt) *= 1. / 1.;
1965
1966
2/4
✓ Branch 2 taken 37109 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 37109 times.
✗ Branch 6 not taken.
 37109 F.middleRows<3>(KFAccBias) *= SCALE_FACTOR_ACCELERATION; // 𝛿f' [mg / s] = 1e3 / g * [m / s^3]
1967
2/4
✓ Branch 2 taken 37109 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 37109 times.
✗ Branch 6 not taken.
 37109 F.middleCols<3>(KFAccBias) *= 1. / SCALE_FACTOR_ACCELERATION;
1968
1969
2/4
✓ Branch 2 taken 37109 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 37109 times.
✗ Branch 6 not taken.
 37109 F.middleRows<3>(KFGyrBias) *= SCALE_FACTOR_ANGULAR_RATE; // 𝛿ω' [mrad / s^2] = 1e3 * [rad / s^2]
1970
2/4
✓ Branch 2 taken 37109 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 37109 times.
✗ Branch 6 not taken.
 37109 F.middleCols<3>(KFGyrBias) *= 1. / SCALE_FACTOR_ANGULAR_RATE;
1971
1972 74218 return F;
1973 ✗ }
1974
1975 NAV::KeyedMatrix<double, NAV::LooselyCoupledKF::KFStates, NAV::LooselyCoupledKF::KFStates, NAV::LooselyCoupledKF::KFStates_COUNT, NAV::LooselyCoupledKF::KFStates_COUNT>
1976 12888 NAV::LooselyCoupledKF::e_systemMatrix_F(const Eigen::Quaterniond& e_Quat_b,
1977 const Eigen::Vector3d& b_specForce_ib,
1978 const Eigen::Vector3d& e_position,
1979 const Eigen::Vector3d& e_gravitation,
1980 double r_eS_e,
1981 const Eigen::Vector3d& e_omega_ie,
1982 const Eigen::Vector3d& tau_bad,
1983 const Eigen::Vector3d& tau_bgd) const
1984 {
1985
3/6
✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12888 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12888 times.
✗ Branch 8 not taken.
 12888 Eigen::Vector3d beta_bad = 1. / tau_bad.array(); // Gauss-Markov constant for the accelerometer 𝛽 = 1 / 𝜏 (𝜏 correlation length)
1986
3/6
✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12888 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12888 times.
✗ Branch 8 not taken.
 12888 Eigen::Vector3d beta_bgd = 1. / tau_bgd.array(); // Gauss-Markov constant for the gyroscope 𝛽 = 1 / 𝜏 (𝜏 correlation length)
1987
1988 // System matrix 𝐅
1989 // Math: \mathbf{F}^e = \begin{pmatrix} \mathbf{F}_{\dot{\psi},\psi}^n & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{C}_b^e \\ \mathbf{F}_{\delta \dot{v},\psi}^n & \mathbf{F}_{\delta \dot{v},\delta v}^n & \mathbf{F}_{\delta \dot{v},\delta r}^n & \mathbf{C}_b^e & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{F}_{\delta \dot{r},\delta v}^n & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \vee -\mathbf{\beta} & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \vee -\mathbf{\beta} \end{pmatrix}
1990 KeyedMatrix<double, KFStates, KFStates, NAV::LooselyCoupledKF::KFStates_COUNT, NAV::LooselyCoupledKF::KFStates_COUNT>
1991
2/4
✓ Branch 2 taken 12888 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12888 times.
✗ Branch 6 not taken.
 12888 F(Eigen::Matrix<double, NAV::LooselyCoupledKF::KFStates_COUNT, NAV::LooselyCoupledKF::KFStates_COUNT>::Zero(), States);
1992
1993
4/8
✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12888 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 12888 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 12888 times.
✗ Branch 13 not taken.
 12888 F.block<3>(KFAtt, KFAtt) = e_F_dpsi_dpsi(e_omega_ie.z());
1994
4/8
✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12888 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 12888 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 12888 times.
✗ Branch 13 not taken.
 12888 F.block<3>(KFAtt, KFGyrBias) = e_F_dpsi_dw(e_Quat_b.toRotationMatrix());
1995
4/8
✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12888 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 12888 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 12888 times.
✗ Branch 13 not taken.
 12888 F.block<3>(KFVel, KFAtt) = e_F_dv_dpsi(e_Quat_b * b_specForce_ib);
1996
4/8
✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12888 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 12888 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 12888 times.
✗ Branch 13 not taken.
 12888 F.block<3>(KFVel, KFVel) = e_F_dv_dv<double>(e_omega_ie.z());
1997
3/6
✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 12888 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 12888 times.
✗ Branch 10 not taken.
 12888 F.block<3>(KFVel, KFPos) = e_F_dv_dr(e_position, e_gravitation, r_eS_e, e_omega_ie);
1998
4/8
✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12888 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 12888 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 12888 times.
✗ Branch 13 not taken.
 12888 F.block<3>(KFVel, KFAccBias) = e_F_dv_df_b(e_Quat_b.toRotationMatrix());
1999
3/6
✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 12888 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 12888 times.
✗ Branch 10 not taken.
 12888 F.block<3>(KFPos, KFVel) = e_F_dr_dv<double>();
2000
2/2
✓ Branch 0 taken 6444 times.
✓ Branch 1 taken 6444 times.
 12888 if (_qCalculationAlgorithm == QCalculationAlgorithm::VanLoan)
2001 {
2002
5/14
✗ Branch 0 not taken.
✓ Branch 1 taken 6444 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 9 taken 6444 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 6444 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 6444 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 6444 times.
✗ Branch 21 not taken.
 6444 F.block<3>(KFAccBias, KFAccBias) = e_F_df_df(_randomProcessAccel == RandomProcess::RandomWalk ? Eigen::Vector3d::Zero() : beta_bad);
2003
5/14
✗ Branch 0 not taken.
✓ Branch 1 taken 6444 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 9 taken 6444 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 6444 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 6444 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 6444 times.
✗ Branch 21 not taken.
 6444 F.block<3>(KFGyrBias, KFGyrBias) = e_F_dw_dw(_randomProcessGyro == RandomProcess::RandomWalk ? Eigen::Vector3d::Zero() : beta_bgd);
2004 }
2005
2006
2/4
✓ Branch 2 taken 12888 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12888 times.
✗ Branch 6 not taken.
 12888 F.middleRows<3>(KFAtt) *= SCALE_FACTOR_ATTITUDE; // 𝜓' [deg / s] = 180/π * ... [rad / s]
2007
2/4
✓ Branch 2 taken 12888 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12888 times.
✗ Branch 6 not taken.
 12888 F.middleCols<3>(KFAtt) *= 1. / SCALE_FACTOR_ATTITUDE;
2008
2009 // F.middleRows<3>(Vel) *= 1.; // 𝛿v' [m / s^2] = 1 * [m / s^2]
2010 // F.middleCols<3>(Vel) *= 1. / 1.;
2011
2012 // F.middleRows<3>(Pos) *= 1.; // 𝛿r' [m / s] = 1 * [m / s]
2013 // F.middleCols<3>(Pos) *= 1. / 1.;
2014
2015
2/4
✓ Branch 2 taken 12888 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12888 times.
✗ Branch 6 not taken.
 12888 F.middleRows<3>(KFAccBias) *= SCALE_FACTOR_ACCELERATION; // 𝛿f' [mg / s] = 1e3 / g * [m / s^3]
2016
2/4
✓ Branch 2 taken 12888 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12888 times.
✗ Branch 6 not taken.
 12888 F.middleCols<3>(KFAccBias) *= 1. / SCALE_FACTOR_ACCELERATION;
2017
2018
2/4
✓ Branch 2 taken 12888 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12888 times.
✗ Branch 6 not taken.
 12888 F.middleRows<3>(KFGyrBias) *= SCALE_FACTOR_ANGULAR_RATE; // 𝛿ω' [mrad / s^2] = 1e3 * [rad / s^2]
2019
2/4
✓ Branch 2 taken 12888 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12888 times.
✗ Branch 6 not taken.
 12888 F.middleCols<3>(KFGyrBias) *= 1. / SCALE_FACTOR_ANGULAR_RATE;
2020
2021 25776 return F;
2022 ✗ }
2023
2024 // ###########################################################################################################
2025 // Noise input matrix 𝐆 & Noise scale matrix 𝐖
2026 // System noise covariance matrix 𝐐
2027 // ###########################################################################################################
2028
2029 NAV::KeyedMatrix<double, NAV::LooselyCoupledKF::KFStates, NAV::LooselyCoupledKF::KFStates, NAV::LooselyCoupledKF::KFStates_COUNT, NAV::LooselyCoupledKF::KFStates_COUNT>
2030 37109 NAV::LooselyCoupledKF::noiseInputMatrix_G(const Eigen::Quaterniond& ien_Quat_b)
2031 {
2032 // DCM matrix from body to navigation frame
2033
1/2
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
 37109 Eigen::Matrix3d ien_Dcm_b = ien_Quat_b.toRotationMatrix();
2034
2035 // Math: \mathbf{G}_{a} = \begin{bmatrix} -\mathbf{C}_b^{i,e,n} & 0 & 0 & 0 \\ 0 & \mathbf{C}_b^{i,e,n} & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & \mathbf{I}_3 & 0 \\ 0 & 0 & 0 & \mathbf{I}_3 \end{bmatrix}
2036 KeyedMatrix<double, KFStates, KFStates, NAV::LooselyCoupledKF::KFStates_COUNT, NAV::LooselyCoupledKF::KFStates_COUNT>
2037
2/4
✓ Branch 3 taken 37109 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 37109 times.
✗ Branch 7 not taken.
 37109 G(Eigen::Matrix<double, NAV::LooselyCoupledKF::KFStates_COUNT, NAV::LooselyCoupledKF::KFStates_COUNT>::Zero(), States, States);
2038
2039
3/6
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 37109 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 37109 times.
✗ Branch 10 not taken.
 37109 G.block<3>(KFAtt, KFAtt) = SCALE_FACTOR_ATTITUDE * ien_Dcm_b;
2040
2/4
✓ Branch 3 taken 37109 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 37109 times.
✗ Branch 7 not taken.
 37109 G.block<3>(KFVel, KFVel) = ien_Dcm_b;
2041
4/8
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 37109 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 37109 times.
✗ Branch 13 not taken.
 37109 G.block<3>(KFAccBias, KFAccBias) = SCALE_FACTOR_ACCELERATION * Eigen::Matrix3d::Identity();
2042
4/8
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 37109 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 37109 times.
✗ Branch 13 not taken.
 37109 G.block<3>(KFGyrBias, KFGyrBias) = SCALE_FACTOR_ANGULAR_RATE * Eigen::Matrix3d::Identity();
2043
1/2
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
 37109 G(KFStates::HeightBias, KFStates::HeightBias) = 1.0;
2044
1/2
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
 37109 G(KFStates::HeightScale, KFStates::HeightScale) = 1.0;
2045 74218 return G;
2046 ✗ }
2047
2048 Eigen::Matrix<double, NAV::LooselyCoupledKF::KFStates_COUNT, NAV::LooselyCoupledKF::KFStates_COUNT>
2049 37109 NAV::LooselyCoupledKF::noiseScaleMatrix_W(const Eigen::Vector3d& sigma_ra, const Eigen::Vector3d& sigma_rg,
2050 const Eigen::Vector3d& sigma_bad, const Eigen::Vector3d& sigma_bgd,
2051 const Eigen::Vector3d& tau_bad, const Eigen::Vector3d& tau_bgd,
2052 const double& sigma_heightBias, const double& sigma_heightScale)
2053 {
2054 Eigen::Matrix<double, NAV::LooselyCoupledKF::KFStates_COUNT, NAV::LooselyCoupledKF::KFStates_COUNT> W =
2055
2/4
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.
 37109 Eigen::Matrix<double, NAV::LooselyCoupledKF::KFStates_COUNT, NAV::LooselyCoupledKF::KFStates_COUNT>::Zero();
2056
2057
4/8
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 37109 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 37109 times.
✗ Branch 11 not taken.
 37109 W.diagonal() << sigma_rg.array().square(),
2058
3/6
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 37109 times.
✗ Branch 8 not taken.
 37109 sigma_ra.array().square(),
2059
2/4
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.
 37109 Eigen::Vector3d::Zero(),
2060
10/18
✓ Branch 0 taken 24221 times.
✓ Branch 1 taken 12888 times.
✓ Branch 3 taken 24221 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 24221 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 24221 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 12888 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 12888 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 12888 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 12888 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 37109 times.
✗ Branch 25 not taken.
 37109 _randomProcessAccel == RandomProcess::RandomWalk ? sigma_bad.array().square() : psd2BiasGaussMarkov(sigma_bad.array().square(), tau_bad), // S_bad
2061
10/18
✓ Branch 0 taken 24221 times.
✓ Branch 1 taken 12888 times.
✓ Branch 3 taken 24221 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 24221 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 24221 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 12888 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 12888 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 12888 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 12888 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 37109 times.
✗ Branch 25 not taken.
 37109 _randomProcessGyro == RandomProcess::RandomWalk ? sigma_bgd.array().square() : psd2BiasGaussMarkov(sigma_bgd.array().square(), tau_bgd), // S_bgd
2062
1/2
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
 37109 sigma_heightBias * sigma_heightBias,
2063
1/2
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
 37109 sigma_heightScale * sigma_heightScale;
2064
2065 37109 return W;
2066 }
2067
2068 NAV::KeyedMatrix<double, NAV::LooselyCoupledKF::KFStates, NAV::LooselyCoupledKF::KFStates, NAV::LooselyCoupledKF::KFStates_COUNT, NAV::LooselyCoupledKF::KFStates_COUNT>
2069 6444 NAV::LooselyCoupledKF::n_systemNoiseCovarianceMatrix_Q(const Eigen::Vector3d& sigma2_ra, const Eigen::Vector3d& sigma2_rg,
2070 const Eigen::Vector3d& sigma2_bad, const Eigen::Vector3d& sigma2_bgd,
2071 const double& sigma_heightBias, const double& sigma_heightScale,
2072 const Eigen::Vector3d& tau_bad, const Eigen::Vector3d& tau_bgd,
2073 const Eigen::Matrix3d& n_F_21, const Eigen::Matrix3d& T_rn_p,
2074 const Eigen::Matrix3d& n_Dcm_b, const double& tau_s)
2075 {
2076 // Math: \mathbf{Q}_{INS}^n = \begin{pmatrix} \mathbf{Q}_{11} & {\mathbf{Q}_{21}^n}^T & {\mathbf{Q}_{31}^n}^T & \mathbf{0}_3 & {\mathbf{Q}_{51}^n}^T \\ \mathbf{Q}_{21}^n & \mathbf{Q}_{22}^n & {\mathbf{Q}_{32}^n}^T & {\mathbf{Q}_{42}^n}^T & \mathbf{Q}_{25}^n \\ \mathbf{Q}_{31}^n & \mathbf{Q}_{32}^n & \mathbf{Q}_{33}^n & \mathbf{Q}_{34}^n & \mathbf{Q}_{35}^n \\ \mathbf{0}_3 & \mathbf{Q}_{42}^n & {\mathbf{Q}_{34}^n}^T & S_{bad}\tau_s\mathbf{I}_3 & \mathbf{0}_3 \\ \mathbf{Q}_{51}^n & \mathbf{Q}_{52}^n & {\mathbf{Q}_{35}^n}^T & \mathbf{0}_3 & S_{bgd}\tau_s\mathbf{I}_3 \end{pmatrix} \qquad \text{P. Groves}\,(14.80)
2077
2/4
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6444 times.
✗ Branch 5 not taken.
 6444 Eigen::Vector3d S_ra = sigma2_ra * tau_s;
2078
2/4
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6444 times.
✗ Branch 5 not taken.
 6444 Eigen::Vector3d S_rg = sigma2_rg * tau_s;
2079
4/8
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6444 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6444 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6444 times.
✗ Branch 11 not taken.
 6444 Eigen::Vector3d S_bad = sigma2_bad.array() / tau_bad.array();
2080
4/8
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6444 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6444 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6444 times.
✗ Branch 11 not taken.
 6444 Eigen::Vector3d S_bgd = sigma2_bgd.array() / tau_bgd.array();
2081
2082
2/4
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6444 times.
✗ Branch 5 not taken.
 6444 Eigen::Matrix3d b_Dcm_n = n_Dcm_b.transpose();
2083
2084 KeyedMatrix<double, KFStates, KFStates, NAV::LooselyCoupledKF::KFStates_COUNT, NAV::LooselyCoupledKF::KFStates_COUNT>
2085
2/4
✓ Branch 3 taken 6444 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
 6444 Q(Eigen::Matrix<double, NAV::LooselyCoupledKF::KFStates_COUNT, NAV::LooselyCoupledKF::KFStates_COUNT>::Zero(), States, States);
2086
3/6
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6444 times.
✗ Branch 10 not taken.
 6444 Q.block<3>(KFAtt, KFAtt) = Q_psi_psi(S_rg, S_bgd, tau_s); // Q_11
2087
3/6
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6444 times.
✗ Branch 10 not taken.
 6444 Q.block<3>(KFVel, KFAtt) = ien_Q_dv_psi(S_rg, S_bgd, n_F_21, tau_s); // Q_21
2088
3/6
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6444 times.
✗ Branch 10 not taken.
 6444 Q.block<3>(KFVel, KFVel) = ien_Q_dv_dv(S_ra, S_bad, S_rg, S_bgd, n_F_21, tau_s); // Q_22
2089
3/6
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6444 times.
✗ Branch 10 not taken.
 6444 Q.block<3>(KFVel, KFGyrBias) = ien_Q_dv_domega(S_bgd, n_F_21, n_Dcm_b, tau_s); // Q_25
2090
3/6
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6444 times.
✗ Branch 10 not taken.
 6444 Q.block<3>(KFPos, KFAtt) = n_Q_dr_psi(S_rg, S_bgd, n_F_21, T_rn_p, tau_s); // Q_31
2091
3/6
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6444 times.
✗ Branch 10 not taken.
 6444 Q.block<3>(KFPos, KFVel) = n_Q_dr_dv(S_ra, S_bad, S_rg, S_bgd, n_F_21, T_rn_p, tau_s); // Q_32
2092
3/6
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6444 times.
✗ Branch 10 not taken.
 6444 Q.block<3>(KFPos, KFPos) = n_Q_dr_dr(S_ra, S_bad, S_rg, S_bgd, n_F_21, T_rn_p, tau_s); // Q_33
2093
3/6
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6444 times.
✗ Branch 10 not taken.
 6444 Q.block<3>(KFPos, KFAccBias) = n_Q_dr_df(S_bgd, T_rn_p, n_Dcm_b, tau_s); // Q_34
2094
3/6
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6444 times.
✗ Branch 10 not taken.
 6444 Q.block<3>(KFPos, KFGyrBias) = n_Q_dr_domega(S_bgd, n_F_21, T_rn_p, n_Dcm_b, tau_s); // Q_35
2095
3/6
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6444 times.
✗ Branch 10 not taken.
 6444 Q.block<3>(KFAccBias, KFVel) = Q_df_dv(S_bad, b_Dcm_n, tau_s); // Q_42
2096
3/6
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6444 times.
✗ Branch 10 not taken.
 6444 Q.block<3>(KFAccBias, KFAccBias) = Q_df_df(S_bad, tau_s); // Q_44
2097
3/6
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6444 times.
✗ Branch 10 not taken.
 6444 Q.block<3>(KFGyrBias, KFAtt) = Q_domega_psi(S_bgd, b_Dcm_n, tau_s); // Q_51
2098
3/6
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6444 times.
✗ Branch 10 not taken.
 6444 Q.block<3>(KFGyrBias, KFGyrBias) = Q_domega_domega(S_bgd, tau_s); // Q_55
2099
2100
4/8
✓ Branch 3 taken 6444 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 11 taken 6444 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 6444 times.
✗ Branch 15 not taken.
 6444 Q.block<3>(KFAtt, KFVel) = Q.block<3>(KFVel, KFAtt).transpose(); // Q_21^T
2101
4/8
✓ Branch 3 taken 6444 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 11 taken 6444 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 6444 times.
✗ Branch 15 not taken.
 6444 Q.block<3>(KFAtt, KFPos) = Q.block<3>(KFPos, KFAtt).transpose(); // Q_31^T
2102
4/8
✓ Branch 3 taken 6444 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 11 taken 6444 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 6444 times.
✗ Branch 15 not taken.
 6444 Q.block<3>(KFVel, KFPos) = Q.block<3>(KFPos, KFVel).transpose(); // Q_32^T
2103
4/8
✓ Branch 3 taken 6444 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 11 taken 6444 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 6444 times.
✗ Branch 15 not taken.
 6444 Q.block<3>(KFAccBias, KFPos) = Q.block<3>(KFPos, KFAccBias).transpose(); // Q_34^T
2104
4/8
✓ Branch 3 taken 6444 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 11 taken 6444 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 6444 times.
✗ Branch 15 not taken.
 6444 Q.block<3>(KFGyrBias, KFVel) = Q.block<3>(KFVel, KFGyrBias).transpose(); // Q_25^T
2105
4/8
✓ Branch 3 taken 6444 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 11 taken 6444 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 6444 times.
✗ Branch 15 not taken.
 6444 Q.block<3>(KFGyrBias, KFPos) = Q.block<3>(KFPos, KFGyrBias).transpose(); // Q_35^T
2106
4/8
✓ Branch 3 taken 6444 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 11 taken 6444 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 6444 times.
✗ Branch 15 not taken.
 6444 Q.block<3>(KFVel, KFAccBias) = Q.block<3>(KFAccBias, KFVel).transpose(); // Q_42^T
2107
4/8
✓ Branch 3 taken 6444 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 11 taken 6444 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 6444 times.
✗ Branch 15 not taken.
 6444 Q.block<3>(KFAtt, KFGyrBias) = Q.block<3>(KFGyrBias, KFAtt).transpose(); // Q_51^T
2108
1/2
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
 6444 Q(KFStates::HeightBias, KFStates::HeightBias) = sigma_heightBias * sigma_heightBias * tau_s;
2109
1/2
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
 6444 Q(KFStates::HeightScale, KFStates::HeightScale) = sigma_heightScale * sigma_heightScale * tau_s;
2110
2111
2/4
✓ Branch 2 taken 6444 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6444 times.
✗ Branch 6 not taken.
 6444 Q.middleRows<3>(KFAtt) *= SCALE_FACTOR_ATTITUDE;
2112
2/4
✓ Branch 2 taken 6444 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6444 times.
✗ Branch 6 not taken.
 6444 Q.middleRows<2>(std::array{ PosLat, PosLon }) *= SCALE_FACTOR_LAT_LON;
2113
2/4
✓ Branch 2 taken 6444 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6444 times.
✗ Branch 6 not taken.
 6444 Q.middleRows<3>(KFAccBias) *= SCALE_FACTOR_ACCELERATION;
2114
2/4
✓ Branch 2 taken 6444 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6444 times.
✗ Branch 6 not taken.
 6444 Q.middleRows<3>(KFGyrBias) *= SCALE_FACTOR_ANGULAR_RATE;
2115
2116
2/4
✓ Branch 2 taken 6444 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6444 times.
✗ Branch 6 not taken.
 6444 Q.middleCols<3>(KFAtt) *= SCALE_FACTOR_ATTITUDE;
2117
2/4
✓ Branch 2 taken 6444 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6444 times.
✗ Branch 6 not taken.
 6444 Q.middleCols<2>(std::array{ PosLat, PosLon }) *= SCALE_FACTOR_LAT_LON;
2118
2/4
✓ Branch 2 taken 6444 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6444 times.
✗ Branch 6 not taken.
 6444 Q.middleCols<3>(KFAccBias) *= SCALE_FACTOR_ACCELERATION;
2119
2/4
✓ Branch 2 taken 6444 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6444 times.
✗ Branch 6 not taken.
 6444 Q.middleCols<3>(KFGyrBias) *= SCALE_FACTOR_ANGULAR_RATE;
2120
2121 12888 return Q;
2122 ✗ }
2123
2124 NAV::KeyedMatrix<double, NAV::LooselyCoupledKF::KFStates, NAV::LooselyCoupledKF::KFStates, NAV::LooselyCoupledKF::KFStates_COUNT, NAV::LooselyCoupledKF::KFStates_COUNT>
2125 6444 NAV::LooselyCoupledKF::e_systemNoiseCovarianceMatrix_Q(const Eigen::Vector3d& sigma2_ra, const Eigen::Vector3d& sigma2_rg,
2126 const Eigen::Vector3d& sigma2_bad, const Eigen::Vector3d& sigma2_bgd,
2127 const double& sigma_heightBias, const double& sigma_heightScale,
2128 const Eigen::Vector3d& tau_bad, const Eigen::Vector3d& tau_bgd,
2129 const Eigen::Matrix3d& e_F_21,
2130 const Eigen::Matrix3d& e_Dcm_b, const double& tau_s)
2131 {
2132 // Math: \mathbf{Q}_{INS}^e = \begin{pmatrix} \mathbf{Q}_{11} & {\mathbf{Q}_{21}^e}^T & {\mathbf{Q}_{31}^e}^T & \mathbf{0}_3 & {\mathbf{Q}_{51}^e}^T \\ \mathbf{Q}_{21}^e & \mathbf{Q}_{22}^e & {\mathbf{Q}_{32}^e}^T & {\mathbf{Q}_{42}^e}^T & \mathbf{Q}_{25}^e \\ \mathbf{Q}_{31}^e & \mathbf{Q}_{32}^e & \mathbf{Q}_{33}^e & \mathbf{Q}_{34}^e & \mathbf{Q}_{35}^e \\ \mathbf{0}_3 & \mathbf{Q}_{42}^e & {\mathbf{Q}_{34}^e}^T & S_{bad}\tau_s\mathbf{I}_3 & \mathbf{0}_3 \\ \mathbf{Q}_{51}^e & \mathbf{Q}_{52}^e & {\mathbf{Q}_{35}^e}^T & \mathbf{0}_3 & S_{bgd}\tau_s\mathbf{I}_3 \end{pmatrix} \qquad \text{P. Groves}\,(14.80)
2133
2/4
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6444 times.
✗ Branch 5 not taken.
 6444 Eigen::Vector3d S_ra = sigma2_ra * tau_s;
2134
2/4
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6444 times.
✗ Branch 5 not taken.
 6444 Eigen::Vector3d S_rg = sigma2_rg * tau_s;
2135
4/8
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6444 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6444 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6444 times.
✗ Branch 11 not taken.
 6444 Eigen::Vector3d S_bad = sigma2_bad.array() / tau_bad.array();
2136
4/8
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6444 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6444 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6444 times.
✗ Branch 11 not taken.
 6444 Eigen::Vector3d S_bgd = sigma2_bgd.array() / tau_bgd.array();
2137
2138
2/4
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6444 times.
✗ Branch 5 not taken.
 6444 Eigen::Matrix3d b_Dcm_e = e_Dcm_b.transpose();
2139
2140 KeyedMatrix<double, KFStates, KFStates, NAV::LooselyCoupledKF::KFStates_COUNT, NAV::LooselyCoupledKF::KFStates_COUNT>
2141
2/4
✓ Branch 3 taken 6444 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
 6444 Q(Eigen::Matrix<double, NAV::LooselyCoupledKF::KFStates_COUNT, NAV::LooselyCoupledKF::KFStates_COUNT>::Zero(), States, States);
2142
3/6
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6444 times.
✗ Branch 10 not taken.
 6444 Q.block<3>(KFAtt, KFAtt) = Q_psi_psi(S_rg, S_bgd, tau_s); // Q_11
2143
3/6
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6444 times.
✗ Branch 10 not taken.
 6444 Q.block<3>(KFVel, KFAtt) = ien_Q_dv_psi(S_rg, S_bgd, e_F_21, tau_s); // Q_21
2144
3/6
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6444 times.
✗ Branch 10 not taken.
 6444 Q.block<3>(KFVel, KFVel) = ien_Q_dv_dv(S_ra, S_bad, S_rg, S_bgd, e_F_21, tau_s); // Q_22
2145
3/6
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6444 times.
✗ Branch 10 not taken.
 6444 Q.block<3>(KFVel, KFGyrBias) = ien_Q_dv_domega(S_bgd, e_F_21, e_Dcm_b, tau_s); // Q_25
2146
3/6
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6444 times.
✗ Branch 10 not taken.
 6444 Q.block<3>(KFPos, KFAtt) = ie_Q_dr_psi(S_rg, S_bgd, e_F_21, tau_s); // Q_31
2147
3/6
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6444 times.
✗ Branch 10 not taken.
 6444 Q.block<3>(KFPos, KFVel) = ie_Q_dr_dv(S_ra, S_bad, S_rg, S_bgd, e_F_21, tau_s); // Q_32
2148
3/6
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6444 times.
✗ Branch 10 not taken.
 6444 Q.block<3>(KFPos, KFPos) = ie_Q_dr_dr(S_ra, S_bad, S_rg, S_bgd, e_F_21, tau_s); // Q_33
2149
3/6
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6444 times.
✗ Branch 10 not taken.
 6444 Q.block<3>(KFPos, KFAccBias) = ie_Q_dr_df(S_bgd, e_Dcm_b, tau_s); // Q_34
2150
3/6
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6444 times.
✗ Branch 10 not taken.
 6444 Q.block<3>(KFPos, KFGyrBias) = ie_Q_dr_domega(S_bgd, e_F_21, e_Dcm_b, tau_s); // Q_35
2151
3/6
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6444 times.
✗ Branch 10 not taken.
 6444 Q.block<3>(KFAccBias, KFVel) = Q_df_dv(S_bad, b_Dcm_e, tau_s); // Q_42
2152
3/6
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6444 times.
✗ Branch 10 not taken.
 6444 Q.block<3>(KFAccBias, KFAccBias) = Q_df_df(S_bad, tau_s); // Q_44
2153
3/6
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6444 times.
✗ Branch 10 not taken.
 6444 Q.block<3>(KFGyrBias, KFAtt) = Q_domega_psi(S_bgd, b_Dcm_e, tau_s); // Q_51
2154
3/6
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6444 times.
✗ Branch 10 not taken.
 6444 Q.block<3>(KFGyrBias, KFGyrBias) = Q_domega_domega(S_bgd, tau_s); // Q_55
2155
2156
4/8
✓ Branch 3 taken 6444 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 11 taken 6444 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 6444 times.
✗ Branch 15 not taken.
 6444 Q.block<3>(KFAtt, KFVel) = Q.block<3>(KFVel, KFAtt).transpose(); // Q_21^T
2157
4/8
✓ Branch 3 taken 6444 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 11 taken 6444 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 6444 times.
✗ Branch 15 not taken.
 6444 Q.block<3>(KFAtt, KFPos) = Q.block<3>(KFPos, KFAtt).transpose(); // Q_31^T
2158
4/8
✓ Branch 3 taken 6444 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 11 taken 6444 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 6444 times.
✗ Branch 15 not taken.
 6444 Q.block<3>(KFVel, KFPos) = Q.block<3>(KFPos, KFVel).transpose(); // Q_32^T
2159
4/8
✓ Branch 3 taken 6444 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 11 taken 6444 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 6444 times.
✗ Branch 15 not taken.
 6444 Q.block<3>(KFAccBias, KFPos) = Q.block<3>(KFPos, KFAccBias).transpose(); // Q_34^T
2160
4/8
✓ Branch 3 taken 6444 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 11 taken 6444 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 6444 times.
✗ Branch 15 not taken.
 6444 Q.block<3>(KFGyrBias, KFVel) = Q.block<3>(KFVel, KFGyrBias).transpose(); // Q_25^T
2161
4/8
✓ Branch 3 taken 6444 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 11 taken 6444 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 6444 times.
✗ Branch 15 not taken.
 6444 Q.block<3>(KFGyrBias, KFPos) = Q.block<3>(KFPos, KFGyrBias).transpose(); // Q_35^T
2162
4/8
✓ Branch 3 taken 6444 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 11 taken 6444 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 6444 times.
✗ Branch 15 not taken.
 6444 Q.block<3>(KFVel, KFAccBias) = Q.block<3>(KFAccBias, KFVel).transpose(); // Q_42^T
2163
4/8
✓ Branch 3 taken 6444 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6444 times.
✗ Branch 7 not taken.
✓ Branch 11 taken 6444 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 6444 times.
✗ Branch 15 not taken.
 6444 Q.block<3>(KFAtt, KFGyrBias) = Q.block<3>(KFGyrBias, KFAtt).transpose(); // Q_51^T
2164
1/2
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
 6444 Q(KFStates::HeightBias, KFStates::HeightBias) = sigma_heightBias * sigma_heightBias * tau_s;
2165
1/2
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
 6444 Q(KFStates::HeightScale, KFStates::HeightScale) = sigma_heightScale * sigma_heightScale * tau_s;
2166
2167
2/4
✓ Branch 2 taken 6444 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6444 times.
✗ Branch 6 not taken.
 6444 Q.middleRows<3>(KFAtt) *= SCALE_FACTOR_ATTITUDE;
2168
2/4
✓ Branch 2 taken 6444 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6444 times.
✗ Branch 6 not taken.
 6444 Q.middleRows<3>(KFAccBias) *= SCALE_FACTOR_ACCELERATION;
2169
2/4
✓ Branch 2 taken 6444 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6444 times.
✗ Branch 6 not taken.
 6444 Q.middleRows<3>(KFGyrBias) *= SCALE_FACTOR_ANGULAR_RATE;
2170
2171
2/4
✓ Branch 2 taken 6444 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6444 times.
✗ Branch 6 not taken.
 6444 Q.middleCols<3>(KFAtt) *= SCALE_FACTOR_ATTITUDE;
2172
2/4
✓ Branch 2 taken 6444 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6444 times.
✗ Branch 6 not taken.
 6444 Q.middleCols<3>(KFAccBias) *= SCALE_FACTOR_ACCELERATION;
2173
2/4
✓ Branch 2 taken 6444 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6444 times.
✗ Branch 6 not taken.
 6444 Q.middleCols<3>(KFGyrBias) *= SCALE_FACTOR_ANGULAR_RATE;
2174
2175 12888 return Q;
2176 ✗ }
2177
2178 // ###########################################################################################################
2179 // Error covariance matrix P
2180 // ###########################################################################################################
2181
2182 NAV::KeyedMatrix<double, NAV::LooselyCoupledKF::KFStates, NAV::LooselyCoupledKF::KFStates, NAV::LooselyCoupledKF::KFStates_COUNT, NAV::LooselyCoupledKF::KFStates_COUNT>
2183 51 NAV::LooselyCoupledKF::initialErrorCovarianceMatrix_P0(const Eigen::Vector3d& variance_angles,
2184 const Eigen::Vector3d& variance_vel,
2185 const Eigen::Vector3d& variance_pos,
2186 const Eigen::Vector3d& variance_accelBias,
2187 const Eigen::Vector3d& variance_gyroBias,
2188 const double& variance_heightBias,
2189 const double& variance_heightScale) const
2190 {
2191
3/4
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 27 times.
✓ Branch 4 taken 24 times.
 51 double scaleFactorPosition = _inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::NED ? SCALE_FACTOR_LAT_LON : 1.0;
2192
2193 // 𝐏 Error covariance matrix
2194 Eigen::Matrix<double, NAV::LooselyCoupledKF::KFStates_COUNT, NAV::LooselyCoupledKF::KFStates_COUNT> P =
2195
2/4
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
 51 Eigen::Matrix<double, NAV::LooselyCoupledKF::KFStates_COUNT, NAV::LooselyCoupledKF::KFStates_COUNT>::Zero();
2196
2197
3/6
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 51 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 51 times.
✗ Branch 9 not taken.
 51 P.diagonal() << std::pow(SCALE_FACTOR_ATTITUDE, 2) * variance_angles, // Flight Angles covariance
2198
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 variance_vel, // Velocity covariance
2199
2/4
✓ Branch 2 taken 51 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 51 times.
✗ Branch 6 not taken.
 51 std::pow(scaleFactorPosition, 2) * variance_pos(0), // Latitude/Pos X covariance
2200
2/4
✓ Branch 2 taken 51 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 51 times.
✗ Branch 6 not taken.
 51 std::pow(scaleFactorPosition, 2) * variance_pos(1), // Longitude/Pos Y covariance
2201
2/4
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
 51 variance_pos(2), // Altitude/Pos Z covariance
2202
2/4
✓ Branch 2 taken 51 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 51 times.
✗ Branch 6 not taken.
 51 std::pow(SCALE_FACTOR_ACCELERATION, 2) * variance_accelBias, // Accelerometer Bias covariance
2203
2/4
✓ Branch 2 taken 51 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 51 times.
✗ Branch 6 not taken.
 102 std::pow(SCALE_FACTOR_ANGULAR_RATE, 2) * variance_gyroBias, // Gyroscope Bias covariance
2204
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 variance_heightBias, // Height Bias covariance
2205
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 variance_heightScale; // Height Scale covariance
2206
1/2
✓ Branch 2 taken 51 times.
✗ Branch 3 not taken.
 51 return { P, States };
2207 }
2208
2209 // ###########################################################################################################
2210 // Correction
2211 // ###########################################################################################################
2212
2213 NAV::KeyedMatrix<double, NAV::LooselyCoupledKF::KFMeas, NAV::LooselyCoupledKF::KFStates, 6, NAV::LooselyCoupledKF::KFStates_COUNT>
2214 25517 NAV::LooselyCoupledKF::n_measurementMatrix_H(const Eigen::Matrix3d& T_rn_p, const Eigen::Matrix3d& n_Dcm_b, const Eigen::Vector3d& b_omega_ib, const Eigen::Vector3d& b_leverArm_InsGnss, const Eigen::Matrix3d& n_Omega_ie)
2215 {
2216 // Math: \mathbf{H}_{G,k}^n = \begin{pmatrix} \mathbf{H}_{r1}^n & \mathbf{0}_3 & -\mathbf{I}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\ \mathbf{H}_{v1}^n & -\mathbf{I}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{H}_{v5}^n \end{pmatrix}_k \qquad \text{P. Groves}\,(14.113)
2217 // G denotes GNSS indicated
2218
2219 NAV::KeyedMatrix<double, NAV::LooselyCoupledKF::KFMeas, NAV::LooselyCoupledKF::KFStates, 6, NAV::LooselyCoupledKF::KFStates_COUNT>
2220
2/4
✓ Branch 3 taken 25517 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 25517 times.
✗ Branch 7 not taken.
 25517 H(Eigen::Matrix<double, 6, NAV::LooselyCoupledKF::KFStates_COUNT>::Zero(), Meas, States);
2221
2222 // Math: \mathbf{H}_{r1}^n \approx \mathbf{\hat{T}}_{r(n)}^p \begin{bmatrix} \begin{pmatrix} \mathbf{C}_b^n \mathbf{l}_{ba}^p \end{pmatrix} \wedge \end{bmatrix} \qquad \text{P. Groves}\,(14.114)
2223
5/10
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 25517 times.
✗ Branch 8 not taken.
✓ Branch 12 taken 25517 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 25517 times.
✗ Branch 16 not taken.
 25517 H.block<3>(dPos, KFAtt) = T_rn_p * math::skewSymmetricMatrix(n_Dcm_b * b_leverArm_InsGnss);
2224
4/8
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 25517 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 25517 times.
✗ Branch 13 not taken.
 25517 H.block<3>(dPos, KFPos) = -Eigen::Matrix3d::Identity();
2225 // Math: \mathbf{H}_{v1}^n \approx \begin{bmatrix} \begin{Bmatrix} \mathbf{C}_b^n (\mathbf{\hat{\omega}}_{ib}^b \wedge \mathbf{l}_{ba}^b) - \mathbf{\hat{\Omega}}_{ie}^n \mathbf{C}_b^n \mathbf{l}_{ba}^b \end{Bmatrix} \wedge \end{bmatrix} \qquad \text{P. Groves}\,(14.114)
2226
8/16
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 25517 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 25517 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 25517 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 25517 times.
✗ Branch 17 not taken.
✓ Branch 21 taken 25517 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 25517 times.
✗ Branch 25 not taken.
 25517 H.block<3>(dVel, KFAtt) = math::skewSymmetricMatrix(n_Dcm_b * (b_omega_ib.cross(b_leverArm_InsGnss)) - n_Omega_ie * n_Dcm_b * b_leverArm_InsGnss);
2227
4/8
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 25517 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 25517 times.
✗ Branch 13 not taken.
 25517 H.block<3>(dVel, KFVel) = -Eigen::Matrix3d::Identity();
2228 // Math: \mathbf{H}_{v5}^n = \mathbf{C}_b^n \begin{bmatrix} \mathbf{l}_{ba}^b \wedge \end{bmatrix} \qquad \text{P. Groves}\,(14.114)
2229
4/8
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 25517 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 25517 times.
✗ Branch 13 not taken.
 25517 H.block<3>(dVel, KFGyrBias) = n_Dcm_b * math::skewSymmetricMatrix(b_leverArm_InsGnss);
2230
2231
2/4
✓ Branch 2 taken 25517 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 25517 times.
✗ Branch 6 not taken.
 25517 H.middleRows<2>(std::array{ dPosLat, dPosLon }) *= SCALE_FACTOR_LAT_LON;
2232
2233
2/4
✓ Branch 2 taken 25517 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 25517 times.
✗ Branch 6 not taken.
 25517 H.middleCols<3>(KFAtt) *= 1. / SCALE_FACTOR_ATTITUDE;
2234
2/4
✓ Branch 2 taken 25517 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 25517 times.
✗ Branch 6 not taken.
 25517 H.middleCols<2>(std::array{ PosLat, PosLon }) *= 1. / SCALE_FACTOR_LAT_LON;
2235 // H.middleCols<3>(AccBias) *= 1. / SCALE_FACTOR_ACCELERATION; // Only zero elements
2236
2/4
✓ Branch 2 taken 25517 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 25517 times.
✗ Branch 6 not taken.
 25517 H.middleCols<3>(KFGyrBias) *= 1. / SCALE_FACTOR_ANGULAR_RATE;
2237
2238 25517 return H;
2239 ✗ }
2240
2241 NAV::KeyedMatrix<double, NAV::LooselyCoupledKF::KFMeas, NAV::LooselyCoupledKF::KFStates, 1, NAV::LooselyCoupledKF::KFStates_COUNT>
2242 ✗ NAV::LooselyCoupledKF::n_measurementMatrix_H(const double& height, const double& scale)
2243 {
2244 NAV::KeyedMatrix<double, NAV::LooselyCoupledKF::KFMeas, NAV::LooselyCoupledKF::KFStates, 1, NAV::LooselyCoupledKF::KFStates_COUNT>
2245 ✗ H(Eigen::Matrix<double, 1, NAV::LooselyCoupledKF::KFStates_COUNT>::Zero(), std::array{ KFMeas::dHgt }, States);
2246
2247 ✗ H(KFMeas::dHgt, PosAlt) = -scale;
2248 ✗ H(KFMeas::dHgt, HeightBias) = 1.0;
2249 ✗ H(KFMeas::dHgt, HeightScale) = height;
2250
2251 ✗ return H;
2252 ✗ }
2253
2254 NAV::KeyedMatrix<double, NAV::LooselyCoupledKF::KFMeas, NAV::LooselyCoupledKF::KFStates, 6, NAV::LooselyCoupledKF::KFStates_COUNT>
2255 1296 NAV::LooselyCoupledKF::e_measurementMatrix_H(const Eigen::Matrix3d& e_Dcm_b, const Eigen::Vector3d& b_omega_ib, const Eigen::Vector3d& b_leverArm_InsGnss, const Eigen::Matrix3d& e_Omega_ie)
2256 {
2257 // Math: \mathbf{H}_{G,k}^e = \begin{pmatrix} \mathbf{H}_{r1}^e & \mathbf{0}_3 & -\mathbf{I}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\ \mathbf{H}_{v1}^e & -\mathbf{I}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{H}_{v5}^e \end{pmatrix}_k \qquad \text{P. Groves}\,(14.113)
2258 // G denotes GNSS indicated
2259
2260 NAV::KeyedMatrix<double, NAV::LooselyCoupledKF::KFMeas, NAV::LooselyCoupledKF::KFStates, 6, NAV::LooselyCoupledKF::KFStates_COUNT>
2261
2/4
✓ Branch 3 taken 1296 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1296 times.
✗ Branch 7 not taken.
 1296 H(Eigen::Matrix<double, 6, NAV::LooselyCoupledKF::KFStates_COUNT>::Zero(), Meas, States);
2262
2263 // Math: \mathbf{H}_{r1}^e \approx \begin{bmatrix} \begin{pmatrix} \mathbf{C}_b^e \mathbf{l}_{ba}^p \end{pmatrix} \wedge \end{bmatrix} \qquad \text{P. Groves}\,(14.114)
2264
4/8
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1296 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 1296 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1296 times.
✗ Branch 13 not taken.
 1296 H.block<3>(dPos, KFAtt) = math::skewSymmetricMatrix(e_Dcm_b * b_leverArm_InsGnss);
2265
4/8
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1296 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 1296 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1296 times.
✗ Branch 13 not taken.
 1296 H.block<3>(dPos, KFPos) = -Eigen::Matrix3d::Identity();
2266 // Math: \mathbf{H}_{v1}^e \approx \begin{bmatrix} \begin{Bmatrix} \mathbf{C}_b^e (\mathbf{\hat{\omega}}_{ib}^b \wedge \mathbf{l}_{ba}^b) - \mathbf{\hat{\Omega}}_{ie}^e \mathbf{C}_b^e \mathbf{l}_{ba}^b \end{Bmatrix} \wedge \end{bmatrix} \qquad \text{P. Groves}\,(14.114)
2267
8/16
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1296 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1296 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1296 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1296 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1296 times.
✗ Branch 17 not taken.
✓ Branch 21 taken 1296 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1296 times.
✗ Branch 25 not taken.
 1296 H.block<3>(dVel, KFAtt) = math::skewSymmetricMatrix(e_Dcm_b * (b_omega_ib.cross(b_leverArm_InsGnss)) - e_Omega_ie * e_Dcm_b * b_leverArm_InsGnss);
2268
4/8
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1296 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 1296 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1296 times.
✗ Branch 13 not taken.
 1296 H.block<3>(dVel, KFVel) = -Eigen::Matrix3d::Identity();
2269 // Math: \mathbf{H}_{v5}^e = \mathbf{C}_b^e \begin{bmatrix} \mathbf{l}_{ba}^b \wedge \end{bmatrix} \qquad \text{P. Groves}\,(14.114)
2270
4/8
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1296 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 1296 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1296 times.
✗ Branch 13 not taken.
 1296 H.block<3>(dVel, KFGyrBias) = e_Dcm_b * math::skewSymmetricMatrix(b_leverArm_InsGnss);
2271
2272
2/4
✓ Branch 2 taken 1296 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1296 times.
✗ Branch 6 not taken.
 1296 H.middleCols<3>(KFAtt) *= 1. / SCALE_FACTOR_ATTITUDE;
2273 // H.middleCols<3>(AccBias) *= 1. / SCALE_FACTOR_ACCELERATION; // Only zero elements
2274
2/4
✓ Branch 2 taken 1296 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1296 times.
✗ Branch 6 not taken.
 1296 H.middleCols<3>(KFGyrBias) *= 1. / SCALE_FACTOR_ANGULAR_RATE;
2275
2276 1296 return H;
2277 ✗ }
2278
2279 NAV::KeyedMatrix<double, NAV::LooselyCoupledKF::KFMeas, NAV::LooselyCoupledKF::KFStates, 1, NAV::LooselyCoupledKF::KFStates_COUNT>
2280 ✗ NAV::LooselyCoupledKF::e_measurementMatrix_H(const Eigen::Vector3d& e_positionEstimate, const double& height, const double& scale)
2281 {
2282 NAV::KeyedMatrix<double, NAV::LooselyCoupledKF::KFMeas, NAV::LooselyCoupledKF::KFStates, 1, NAV::LooselyCoupledKF::KFStates_COUNT>
2283 ✗ H(Eigen::Matrix<double, 1, NAV::LooselyCoupledKF::KFStates_COUNT>::Zero(), std::array{ KFMeas::dHgt }, States);
2284
2285 ✗ H(KFMeas::dHgt, KFPos) = -scale * e_positionEstimate.normalized().transpose();
2286 ✗ H(KFMeas::dHgt, HeightBias) = 1.0;
2287 ✗ H(KFMeas::dHgt, HeightScale) = height;
2288
2289 ✗ return H;
2290 ✗ }
2291
2292 NAV::KeyedMatrix<double, NAV::LooselyCoupledKF::KFMeas, NAV::LooselyCoupledKF::KFMeas, 6, 6>
2293 25517 NAV::LooselyCoupledKF::n_measurementNoiseCovariance_R(const std::shared_ptr<const PosVel>& posVelObs,
2294 const Eigen::Vector3d& lla_position,
2295 double R_N,
2296 double R_E) const
2297 {
2298 // GNSS measurement uncertainty for the position (Variance σ²) in [m^2]
2299
2/4
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
 25517 Eigen::Vector3d gnssSigmaSquaredPosition = Eigen::Vector3d::Zero();
2300
2/3
✓ Branch 0 taken 24221 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 1296 times.
 25517 switch (_gnssMeasurementUncertaintyPositionUnit)
2301 {
2302 24221 case GnssMeasurementUncertaintyPositionUnit::meter:
2303
3/6
✓ Branch 1 taken 24221 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24221 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24221 times.
✗ Branch 8 not taken.
 24221 gnssSigmaSquaredPosition = _gnssMeasurementUncertaintyPosition.array().pow(2);
2304 24221 break;
2305 ✗ case GnssMeasurementUncertaintyPositionUnit::meter2:
2306 ✗ gnssSigmaSquaredPosition = _gnssMeasurementUncertaintyPosition;
2307 ✗ break;
2308 }
2309 // GNSS measurement uncertainty for the velocity (Variance σ²) in [m^2/s^2]
2310
2/4
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
 25517 Eigen::Vector3d gnssSigmaSquaredVelocity = Eigen::Vector3d::Zero();
2311
1/3
✓ Branch 0 taken 25517 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 25517 switch (_gnssMeasurementUncertaintyVelocityUnit)
2312 {
2313 25517 case GnssMeasurementUncertaintyVelocityUnit::m_s:
2314
3/6
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 25517 times.
✗ Branch 8 not taken.
 25517 gnssSigmaSquaredVelocity = _gnssMeasurementUncertaintyVelocity.array().pow(2);
2315 25517 break;
2316 ✗ case GnssMeasurementUncertaintyVelocityUnit::m2_s2:
2317 ✗ gnssSigmaSquaredVelocity = _gnssMeasurementUncertaintyVelocity;
2318 ✗ break;
2319 }
2320
2321
2/4
✓ Branch 2 taken 25517 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 25517 times.
✗ Branch 6 not taken.
 25517 KeyedMatrix<double, KFMeas, KFMeas, 6, 6> R(Eigen::Matrix<double, 6, 6>::Zero(), Meas);
2322
2323
1/2
✓ Branch 0 taken 1296 times.
✗ Branch 1 not taken.
 1296 if (!_gnssMeasurementUncertaintyPositionOverride && !_gnssMeasurementUncertaintyVelocityOverride
2324
4/10
✓ Branch 0 taken 1296 times.
✓ Branch 1 taken 24221 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 1296 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✓ Branch 18 taken 25517 times.
 26813 && posVelObs->n_CovarianceMatrix() && posVelObs->n_CovarianceMatrix()->get().hasRows(Keys::PosVel<Keys::MotionModelKey>))
2325 {
2326 ✗ R(all, all) = posVelObs->n_CovarianceMatrix()->get()(Keys::PosVel<Keys::MotionModelKey>, Keys::PosVel<Keys::MotionModelKey>);
2327 }
2328 51034 else if (!_gnssMeasurementUncertaintyPositionOverride
2329
4/10
✓ Branch 0 taken 1296 times.
✓ Branch 1 taken 24221 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 1296 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✓ Branch 18 taken 25517 times.
 25517 && posVelObs->n_CovarianceMatrix() && posVelObs->n_CovarianceMatrix()->get().hasRows(Keys::Pos<Keys::MotionModelKey>))
2330 {
2331 ✗ R.block<3>(dPos, dPos) = posVelObs->n_CovarianceMatrix()->get()(Keys::Pos<Keys::MotionModelKey>, Keys::Pos<Keys::MotionModelKey>);
2332 ✗ R.block<3>(dVel, dVel).diagonal() = gnssSigmaSquaredVelocity;
2333 }
2334 51034 else if (!_gnssMeasurementUncertaintyVelocityOverride
2335
4/10
✓ Branch 0 taken 1296 times.
✓ Branch 1 taken 24221 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 1296 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✓ Branch 18 taken 25517 times.
 25517 && posVelObs->n_CovarianceMatrix() && posVelObs->n_CovarianceMatrix()->get().hasRows(Keys::Vel<Keys::MotionModelKey>))
2336 {
2337 ✗ R.block<3>(dPos, dPos).diagonal() = gnssSigmaSquaredPosition;
2338 ✗ R.block<3>(dVel, dVel) = posVelObs->n_CovarianceMatrix()->get()(Keys::Vel<Keys::MotionModelKey>, Keys::Vel<Keys::MotionModelKey>);
2339 }
2340 else // if (_gnssMeasurementUncertaintyPositionOverride && _gnssMeasurementUncertaintyVelocityOverride)
2341 {
2342
3/6
✓ Branch 3 taken 25517 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 25517 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 25517 times.
✗ Branch 10 not taken.
 25517 R.block<3>(dPos, dPos).diagonal() = gnssSigmaSquaredPosition;
2343
3/6
✓ Branch 3 taken 25517 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 25517 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 25517 times.
✗ Branch 10 not taken.
 25517 R.block<3>(dVel, dVel).diagonal() = gnssSigmaSquaredVelocity;
2344 }
2345
2346 // transform NED covariance into LLA covariance
2347
2/4
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
 25517 Eigen::Matrix6d J = Eigen::Matrix6d::Identity();
2348
5/10
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 25517 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 25517 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 25517 times.
✗ Branch 14 not taken.
 25517 J.topLeftCorner<3, 3>() = n_F_dr_dv(lla_position.x(), lla_position.z(), R_N, R_E);
2349
4/8
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 25517 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 25517 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 25517 times.
✗ Branch 13 not taken.
 25517 R(all, all) = J * R(all, all) * J.transpose();
2350
2351 // scale units
2352
2/4
✓ Branch 2 taken 25517 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 25517 times.
✗ Branch 6 not taken.
 25517 R.middleRows<2>(std::array{ dPosLat, dPosLon }) *= SCALE_FACTOR_LAT_LON;
2353
2/4
✓ Branch 2 taken 25517 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 25517 times.
✗ Branch 6 not taken.
 25517 R.middleCols<2>(std::array{ dPosLat, dPosLon }) *= SCALE_FACTOR_LAT_LON;
2354
2355 51034 return R;
2356 }
2357
2358 NAV::KeyedMatrix<double, NAV::LooselyCoupledKF::KFMeas, NAV::LooselyCoupledKF::KFMeas, 1, 1>
2359 ✗ NAV::LooselyCoupledKF::n_measurementNoiseCovariance_R(const double& baroVarianceHeight)
2360 {
2361 ✗ KeyedMatrix<double, KFMeas, KFMeas, 1, 1> R(Eigen::Matrix<double, 1, 1>::Zero(), std::array{ KFMeas::dHgt });
2362 ✗ R(KFMeas::dHgt, KFMeas::dHgt) = baroVarianceHeight;
2363
2364 ✗ return R;
2365 ✗ }
2366
2367 NAV::KeyedMatrix<double, NAV::LooselyCoupledKF::KFMeas, NAV::LooselyCoupledKF::KFMeas, 6, 6>
2368 1296 NAV::LooselyCoupledKF::e_measurementNoiseCovariance_R(const std::shared_ptr<const PosVel>& posVelObs) const
2369 {
2370 // GNSS measurement uncertainty for the position (Variance σ²) in [m^2]
2371
2/4
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1296 times.
✗ Branch 5 not taken.
 1296 Eigen::Vector3d gnssSigmaSquaredPosition = Eigen::Vector3d::Zero();
2372
1/3
✗ Branch 0 not taken.
✗ Branch 1 not taken.
✓ Branch 2 taken 1296 times.
 1296 switch (_gnssMeasurementUncertaintyPositionUnit)
2373 {
2374 ✗ case GnssMeasurementUncertaintyPositionUnit::meter:
2375 ✗ gnssSigmaSquaredPosition = _gnssMeasurementUncertaintyPosition.array().pow(2);
2376 ✗ break;
2377 ✗ case GnssMeasurementUncertaintyPositionUnit::meter2:
2378 ✗ gnssSigmaSquaredPosition = _gnssMeasurementUncertaintyPosition;
2379 ✗ break;
2380 }
2381 // GNSS measurement uncertainty for the velocity (Variance σ²) in [m^2/s^2]
2382
2/4
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1296 times.
✗ Branch 5 not taken.
 1296 Eigen::Vector3d gnssSigmaSquaredVelocity = Eigen::Vector3d::Zero();
2383
1/3
✓ Branch 0 taken 1296 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 1296 switch (_gnssMeasurementUncertaintyVelocityUnit)
2384 {
2385 1296 case GnssMeasurementUncertaintyVelocityUnit::m_s:
2386
3/6
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1296 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1296 times.
✗ Branch 8 not taken.
 1296 gnssSigmaSquaredVelocity = _gnssMeasurementUncertaintyVelocity.array().pow(2);
2387 1296 break;
2388 ✗ case GnssMeasurementUncertaintyVelocityUnit::m2_s2:
2389 ✗ gnssSigmaSquaredVelocity = _gnssMeasurementUncertaintyVelocity;
2390 ✗ break;
2391 }
2392
2393
2/4
✓ Branch 2 taken 1296 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1296 times.
✗ Branch 6 not taken.
 1296 KeyedMatrix<double, KFMeas, KFMeas, 6, 6> R(Eigen::Matrix<double, 6, 6>::Zero(), Meas);
2394
2395
1/2
✓ Branch 0 taken 1296 times.
✗ Branch 1 not taken.
 1296 if (!_gnssMeasurementUncertaintyPositionOverride && !_gnssMeasurementUncertaintyVelocityOverride
2396
3/10
✓ Branch 0 taken 1296 times.
✗ Branch 1 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 1296 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✓ Branch 18 taken 1296 times.
 2592 && posVelObs->e_CovarianceMatrix() && posVelObs->e_CovarianceMatrix()->get().hasRows(Keys::PosVel<Keys::MotionModelKey>))
2397 {
2398 ✗ R(all, all) = posVelObs->e_CovarianceMatrix()->get()(Keys::PosVel<Keys::MotionModelKey>, Keys::PosVel<Keys::MotionModelKey>);
2399 }
2400 2592 else if (!_gnssMeasurementUncertaintyPositionOverride
2401
3/10
✓ Branch 0 taken 1296 times.
✗ Branch 1 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 1296 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✓ Branch 18 taken 1296 times.
 1296 && posVelObs->e_CovarianceMatrix() && posVelObs->e_CovarianceMatrix()->get().hasRows(Keys::Pos<Keys::MotionModelKey>))
2402 {
2403 ✗ R.block<3>(dPos, dPos) = posVelObs->e_CovarianceMatrix()->get()(Keys::Pos<Keys::MotionModelKey>, Keys::Pos<Keys::MotionModelKey>);
2404 ✗ R.block<3>(dVel, dVel).diagonal() = gnssSigmaSquaredVelocity;
2405 }
2406 2592 else if (!_gnssMeasurementUncertaintyVelocityOverride
2407
3/10
✓ Branch 0 taken 1296 times.
✗ Branch 1 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 1296 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✓ Branch 18 taken 1296 times.
 1296 && posVelObs->e_CovarianceMatrix() && posVelObs->e_CovarianceMatrix()->get().hasRows(Keys::Vel<Keys::MotionModelKey>))
2408 {
2409 ✗ R.block<3>(dPos, dPos).diagonal() = gnssSigmaSquaredPosition;
2410 ✗ R.block<3>(dVel, dVel) = posVelObs->e_CovarianceMatrix()->get()(Keys::Vel<Keys::MotionModelKey>, Keys::Vel<Keys::MotionModelKey>);
2411 }
2412 else // if (_gnssMeasurementUncertaintyPositionOverride && _gnssMeasurementUncertaintyVelocityOverride)
2413 {
2414
3/6
✓ Branch 3 taken 1296 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1296 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1296 times.
✗ Branch 10 not taken.
 1296 R.block<3>(dPos, dPos).diagonal() = gnssSigmaSquaredPosition;
2415
3/6
✓ Branch 3 taken 1296 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1296 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1296 times.
✗ Branch 10 not taken.
 1296 R.block<3>(dVel, dVel).diagonal() = gnssSigmaSquaredVelocity;
2416 }
2417
2418 2592 return R;
2419 }
2420
2421 NAV::KeyedVector<double, NAV::LooselyCoupledKF::KFMeas, 6>
2422 25517 NAV::LooselyCoupledKF::n_measurementInnovation_dz(const Eigen::Vector3d& lla_positionMeasurement, const Eigen::Vector3d& lla_positionEstimate,
2423 const Eigen::Vector3d& n_velocityMeasurement, const Eigen::Vector3d& n_velocityEstimate,
2424 const Eigen::Matrix3d& T_rn_p, const Eigen::Quaterniond& n_Quat_b, const Eigen::Vector3d& b_leverArm_InsGnss,
2425 const Eigen::Vector3d& b_omega_ib, const Eigen::Matrix3d& n_Omega_ie)
2426 {
2427 // Math: \delta\mathbf{z}_{G,k}^{n-} = \begin{pmatrix} \mathbf{\hat{p}}_{aG} - \mathbf{\hat{p}}_b - \mathbf{\hat{T}}_{r(n)}^p \mathbf{C}_b^n \mathbf{l}_{ba}^b \\ \mathbf{\hat{v}}_{eaG}^n - \mathbf{\hat{v}}_{eb}^n - \mathbf{C}_b^n (\mathbf{\hat{\omega}}_{ib}^b \wedge \mathbf{l}_{ba}^b) + \mathbf{\hat{\Omega}}_{ie}^n \mathbf{C}_b^n \mathbf{l}_{ba}^b \end{pmatrix}_k \qquad \text{P. Groves}\,(14.103)
2428
5/10
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 25517 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 25517 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 25517 times.
✗ Branch 14 not taken.
 25517 Eigen::Vector3d deltaLLA = lla_positionMeasurement - lla_positionEstimate - T_rn_p * (n_Quat_b * b_leverArm_InsGnss);
2429
8/16
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 25517 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 25517 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 25517 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 25517 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 25517 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 25517 times.
✗ Branch 23 not taken.
 25517 Eigen::Vector3d deltaVel = n_velocityMeasurement - n_velocityEstimate - n_Quat_b * (b_omega_ib.cross(b_leverArm_InsGnss)) + n_Omega_ie * (n_Quat_b * b_leverArm_InsGnss);
2430
2431
2/4
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
 25517 deltaLLA.topRows<2>() *= SCALE_FACTOR_LAT_LON;
2432
2433
1/2
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
 25517 Eigen::Matrix<double, 6, 1> innovation;
2434
2/4
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
 25517 innovation << deltaLLA, deltaVel;
2435
2436
1/2
✓ Branch 2 taken 25517 times.
✗ Branch 3 not taken.
 25517 return { innovation, Meas };
2437 }
2438
2439 NAV::KeyedVector<double, NAV::LooselyCoupledKF::KFMeas, 1>
2440 ✗ NAV::LooselyCoupledKF::n_measurementInnovation_dz(const double& baroheight, const double& height, const double& heightbias, const double& heightscale)
2441 {
2442 ✗ Eigen::Matrix<double, 1, 1> innovation;
2443 ✗ innovation << baroheight - (height * heightscale + heightbias);
2444
2445 ✗ return { innovation, std::array{ KFMeas::dHgt } };
2446 }
2447
2448 NAV::KeyedVector<double, NAV::LooselyCoupledKF::KFMeas, 6>
2449 1296 NAV::LooselyCoupledKF::e_measurementInnovation_dz(const Eigen::Vector3d& e_positionMeasurement, const Eigen::Vector3d& e_positionEstimate,
2450 const Eigen::Vector3d& e_velocityMeasurement, const Eigen::Vector3d& e_velocityEstimate,
2451 const Eigen::Quaterniond& e_Quat_b, const Eigen::Vector3d& b_leverArm_InsGnss,
2452 const Eigen::Vector3d& b_omega_ib, const Eigen::Matrix3d& e_Omega_ie)
2453 {
2454 // Math: \delta\mathbf{z}_{G,k}^{e-} = \begin{pmatrix} \mathbf{\hat{r}}_{eaG}^e - \mathbf{\hat{r}}_{eb}^e - \mathbf{C}_b^e \mathbf{l}_{ba}^b \\ \mathbf{\hat{v}}_{eaG}^e - \mathbf{\hat{v}}_{eb}^e - \mathbf{C}_b^e (\mathbf{\hat{\omega}}_{ib}^b \wedge \mathbf{l}_{ba}^b) + \mathbf{\hat{\Omega}}_{ie}^e \mathbf{C}_b^e \mathbf{l}_{ba}^b \end{pmatrix}_k \qquad \text{P. Groves}\,(14.102)
2455
4/8
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1296 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1296 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1296 times.
✗ Branch 11 not taken.
 1296 Eigen::Vector3d deltaPos = e_positionMeasurement - e_positionEstimate - e_Quat_b * b_leverArm_InsGnss;
2456
8/16
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1296 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1296 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1296 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1296 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1296 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1296 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1296 times.
✗ Branch 23 not taken.
 1296 Eigen::Vector3d deltaVel = e_velocityMeasurement - e_velocityEstimate - e_Quat_b * (b_omega_ib.cross(b_leverArm_InsGnss)) + e_Omega_ie * (e_Quat_b * b_leverArm_InsGnss);
2457
2458
1/2
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
 1296 Eigen::Matrix<double, 6, 1> innovation;
2459
2/4
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1296 times.
✗ Branch 5 not taken.
 1296 innovation << deltaPos, deltaVel;
2460
2461
1/2
✓ Branch 2 taken 1296 times.
✗ Branch 3 not taken.
 1296 return { innovation, Meas };
2462 }
2463
2464 ✗ std::ostream& operator<<(std::ostream& os, const NAV::LooselyCoupledKF::KFStates& obj)
2465 {
2466 ✗ return os << fmt::format("{}", obj);
2467 }
2468 ✗ std::ostream& operator<<(std::ostream& os, const NAV::LooselyCoupledKF::KFMeas& obj)
2469 {
2470 ✗ return os << fmt::format("{}", obj);
2471 }
2472