INSTINCT Code Coverage Report

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