INSTINCT Code Coverage Report

Directory: src/
File: Nodes/DataProcessor/KalmanFilter/LooselyCoupledKF.cpp
Date: 2025-02-07 16:54:41
Exec Total Coverage
Lines: 575 921 62.4%
Functions: 30 35 85.7%
Branches: 1080 3010 35.9%
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/PosVel.hpp"
12 #include "util/Eigen.hpp"
13 #include <cmath>
14
15 #include <imgui.h>
16 #include <imgui_internal.h>
17 #include "internal/gui/widgets/HelpMarker.hpp"
18 #include "internal/gui/widgets/imgui_ex.hpp"
19 #include "internal/gui/widgets/InputWithUnit.hpp"
20 #include "internal/gui/NodeEditorApplication.hpp"
21
22 #include "internal/FlowManager.hpp"
23 #include "internal/NodeManager.hpp"
24 namespace nm = NAV::NodeManager;
25 #include "NodeRegistry.hpp"
26 #include "Navigation/Constants.hpp"
27 #include "Navigation/Ellipsoid/Ellipsoid.hpp"
28 #include "Navigation/INS/Functions.hpp"
29 #include "Navigation/INS/ProcessNoise.hpp"
30 #include "Navigation/INS/EcefFrame/ErrorEquations.hpp"
31 #include "Navigation/INS/LocalNavFrame/ErrorEquations.hpp"
32 #include "Navigation/Math/Math.hpp"
33 #include "Navigation/Math/VanLoan.hpp"
34 #include "Navigation/Gravity/Gravity.hpp"
35 #include "Navigation/Transformations/Units.hpp"
36 #include "util/Logger.hpp"
37 #include "util/Assert.h"
38
39 #include "NodeData/IMU/ImuObsWDelta.hpp"
40 #include "NodeData/State/PosVelAtt.hpp"
41 #include "NodeData/State/InsGnssLCKFSolution.hpp"
42
43 /// @brief Scale factor to convert the attitude error
44 constexpr double SCALE_FACTOR_ATTITUDE = 180. / M_PI;
45 /// @brief Scale factor to convert the latitude and longitude error
46 constexpr double SCALE_FACTOR_LAT_LON = NAV::InsConst::pseudometre;
47 /// @brief Scale factor to convert the acceleration error
48 constexpr double SCALE_FACTOR_ACCELERATION = 1e3 / NAV::InsConst::G_NORM;
49 /// @brief Scale factor to convert the angular rate error
50 constexpr double SCALE_FACTOR_ANGULAR_RATE = 1e3;
51
52 129 NAV::LooselyCoupledKF::LooselyCoupledKF()
53
32/64
✓ Branch 1 taken 129 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 129 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 129 times.
✗ Branch 9 not taken.
✓ Branch 13 taken 129 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 129 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 129 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 129 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 129 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 129 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 129 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 129 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 129 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 129 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 129 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 129 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 129 times.
✗ Branch 50 not taken.
✓ Branch 52 taken 129 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 129 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 129 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 129 times.
✗ Branch 62 not taken.
✓ Branch 64 taken 129 times.
✗ Branch 65 not taken.
✓ Branch 67 taken 129 times.
✗ Branch 68 not taken.
✓ Branch 70 taken 129 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 129 times.
✗ Branch 74 not taken.
✓ Branch 76 taken 129 times.
✗ Branch 77 not taken.
✓ Branch 79 taken 129 times.
✗ Branch 80 not taken.
✓ Branch 82 taken 129 times.
✗ Branch 83 not taken.
✓ Branch 85 taken 129 times.
✗ Branch 86 not taken.
✓ Branch 88 taken 129 times.
✗ Branch 89 not taken.
✓ Branch 91 taken 129 times.
✗ Branch 92 not taken.
✓ Branch 94 taken 129 times.
✗ Branch 95 not taken.
✓ Branch 97 taken 129 times.
✗ Branch 98 not taken.
 129 : Node(typeStatic())
54 {
55 LOG_TRACE("{}: called", name);
56
57 129 _hasConfig = true;
58 129 _guiConfigDefaultWindowSize = { 822, 936 };
59
60
4/8
✓ Branch 2 taken 129 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 129 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 258 times.
✓ Branch 10 taken 129 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 645 nm::CreateInputPin(
61 this, "ImuObs", Pin::Type::Flow, { NAV::ImuObs::type(), NAV::ImuObsWDelta::type() }, &LooselyCoupledKF::recvImuObservation,
62 ✗ [](const Node* node, const InputPin& inputPin) {
63 49998 const auto* lckf = static_cast<const LooselyCoupledKF*>(node); // NOLINT(cppcoreguidelines-pro-type-static-cast-downcast)
64
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();
65 },
66 1); // Priority 1 ensures, that the IMU obs (prediction) is evaluated before the PosVel obs (update)
67
4/8
✓ Branch 2 taken 129 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 129 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 129 times.
✓ Branch 10 taken 129 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 516 nm::CreateInputPin(
68 this, "PosVel", Pin::Type::Flow, { NAV::PosVel::type() }, &LooselyCoupledKF::recvPosVelObservation,
69 ✗ [](const Node* node, const InputPin& inputPin) {
70 26830 const auto* lckf = static_cast<const LooselyCoupledKF*>(node); // NOLINT(cppcoreguidelines-pro-type-static-cast-downcast)
71
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());
72 },
73 2); // Initially this has higher priority than the IMU obs, to initialize the position from it
74
75
4/8
✓ Branch 2 taken 129 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 129 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 129 times.
✓ Branch 10 taken 129 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 516 nm::CreateOutputPin(this, "PosVelAtt", Pin::Type::Flow, { NAV::InsGnssLCKFSolution::type() });
76 516 }
77
78 292 NAV::LooselyCoupledKF::~LooselyCoupledKF()
79 {
80 LOG_TRACE("{}: called", nameId());
81 292 }
82
83 241 std::string NAV::LooselyCoupledKF::typeStatic()
84 {
85
1/2
✓ Branch 1 taken 241 times.
✗ Branch 2 not taken.
 482 return "INS/GNSS LCKF"; // Loosely-coupled Kalman Filter
86 }
87
88 ✗ std::string NAV::LooselyCoupledKF::type() const
89 {
90 ✗ return typeStatic();
91 }
92
93 112 std::string NAV::LooselyCoupledKF::category()
94 {
95
1/2
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
 224 return "Data Processor";
96 }
97
98 17 void NAV::LooselyCoupledKF::updateExternalPvaInitPin()
99 {
100
3/6
✓ Branch 0 taken 17 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 17 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 17 times.
✗ Branch 6 not taken.
 17 if (_initializeStateOverExternalPin && inputPins.size() <= INPUT_PORT_INDEX_POS_VEL_ATT_INIT)
101 {
102
4/8
✓ 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 taken 17 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
 51 nm::CreateInputPin(
103 this, "Init PVA", Pin::Type::Flow, { NAV::PosVelAtt::type() }, &LooselyCoupledKF::recvPosVelAttInit,
104 nullptr,
105 3);
106 }
107 ✗ else if (!_initializeStateOverExternalPin && inputPins.size() > INPUT_PORT_INDEX_POS_VEL_ATT_INIT)
108 {
109 ✗ nm::DeleteInputPin(inputPins[INPUT_PORT_INDEX_POS_VEL_ATT_INIT]);
110 }
111 34 }
112
113 ✗ void NAV::LooselyCoupledKF::guiConfig()
114 {
115 ✗ float configWidth = 400.0F * gui::NodeEditorApplication::windowFontRatio();
116 ✗ float unitWidth = 150.0F * gui::NodeEditorApplication::windowFontRatio();
117
118 ✗ float taylorOrderWidth = 75.0F * gui::NodeEditorApplication::windowFontRatio();
119
120 ✗ if (ImGui::CollapsingHeader(fmt::format("Initialization##{}", size_t(id)).c_str(), ImGuiTreeNodeFlags_DefaultOpen))
121 {
122 ✗ if (ImGui::Checkbox(fmt::format("Initialize over pin##{}", size_t(id)).c_str(), &_initializeStateOverExternalPin))
123 {
124 ✗ updateExternalPvaInitPin();
125 ✗ flow::ApplyChanges();
126 }
127 ✗ if (!_initializeStateOverExternalPin)
128 {
129 ✗ ImGui::SetNextItemWidth(80 * gui::NodeEditorApplication::windowFontRatio());
130 ✗ if (ImGui::DragDouble(fmt::format("##initalRollPitchYaw(0) {}", size_t(id)).c_str(),
131 _initalRollPitchYaw.data(), 1.0F, -180.0, 180.0, "%.3f °"))
132 {
133 ✗ flow::ApplyChanges();
134 }
135 ✗ ImGui::SameLine();
136 ✗ ImGui::SetNextItemWidth(80 * gui::NodeEditorApplication::windowFontRatio());
137 ✗ if (ImGui::DragDouble(fmt::format("##initalRollPitchYaw(1) {}", size_t(id)).c_str(),
138 ✗ &_initalRollPitchYaw[1], 1.0F, -90.0, 90.0, "%.3f °"))
139 {
140 ✗ flow::ApplyChanges();
141 }
142 ✗ ImGui::SameLine();
143 ✗ ImGui::SetNextItemWidth(80 * gui::NodeEditorApplication::windowFontRatio());
144 ✗ if (ImGui::DragDouble(fmt::format("##initalRollPitchYaw(2) {}", size_t(id)).c_str(),
145 ✗ &_initalRollPitchYaw[2], 1.0, -180.0, 180.0, "%.3f °"))
146 {
147 ✗ flow::ApplyChanges();
148 }
149 ✗ ImGui::SameLine();
150 ✗ ImGui::TextUnformatted("Roll, Pitch, Yaw");
151 }
152 }
153
154 ✗ if (ImGui::CollapsingHeader(fmt::format("IMU Integrator settings##{}", size_t(id)).c_str(), ImGuiTreeNodeFlags_DefaultOpen))
155 {
156 ✗ if (InertialIntegratorGui(std::to_string(size_t(id)).c_str(), _inertialIntegrator))
157 {
158 ✗ flow::ApplyChanges();
159 }
160 ✗ if (inputPins.at(INPUT_PORT_INDEX_IMU).isPinLinked()
161 ✗ && NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(inputPins.at(INPUT_PORT_INDEX_IMU).link.getConnectedPin()->dataIdentifier, { ImuObsWDelta::type() }))
162 {
163 ✗ ImGui::Separator();
164 ✗ if (ImGui::Checkbox(fmt::format("Prefer raw measurements over delta##{}", size_t(id)).c_str(), &_preferAccelerationOverDeltaMeasurements))
165 {
166 ✗ flow::ApplyChanges();
167 }
168 }
169 }
170 ✗ if (ImGui::CollapsingHeader(fmt::format("Kalman Filter settings##{}", size_t(id)).c_str(), ImGuiTreeNodeFlags_DefaultOpen))
171 {
172 ✗ if (_phiCalculationAlgorithm == PhiCalculationAlgorithm::Taylor)
173 {
174 ✗ ImGui::SetNextItemWidth(configWidth - taylorOrderWidth);
175 }
176 else
177 {
178 ✗ ImGui::SetNextItemWidth(configWidth + ImGui::GetStyle().ItemSpacing.x);
179 }
180 ✗ if (auto phiCalculationAlgorithm = static_cast<int>(_phiCalculationAlgorithm);
181 ✗ ImGui::Combo(fmt::format("##Phi calculation algorithm {}", size_t(id)).c_str(), &phiCalculationAlgorithm, "Van Loan\0Taylor\0\0"))
182 {
183 ✗ _phiCalculationAlgorithm = static_cast<decltype(_phiCalculationAlgorithm)>(phiCalculationAlgorithm);
184 ✗ LOG_DEBUG("{}: Phi calculation algorithm changed to {}", nameId(), fmt::underlying(_phiCalculationAlgorithm));
185 ✗ flow::ApplyChanges();
186 }
187
188 ✗ if (_phiCalculationAlgorithm == PhiCalculationAlgorithm::Taylor)
189 {
190 ✗ ImGui::SameLine();
191 ✗ ImGui::SetNextItemWidth(taylorOrderWidth);
192 ✗ if (ImGui::InputIntL(fmt::format("##Phi calculation Taylor Order {}", size_t(id)).c_str(), &_phiCalculationTaylorOrder, 1, 9))
193 {
194 ✗ LOG_DEBUG("{}: Phi calculation Taylor Order changed to {}", nameId(), _phiCalculationTaylorOrder);
195 ✗ flow::ApplyChanges();
196 }
197 }
198 ✗ ImGui::SameLine();
199 ✗ ImGui::SetCursorPosX(ImGui::GetCursorPosX() - ImGui::GetStyle().ItemSpacing.x + ImGui::GetStyle().ItemInnerSpacing.x);
200 ✗ ImGui::Text("Phi calculation algorithm%s", _phiCalculationAlgorithm == PhiCalculationAlgorithm::Taylor ? " (up to order)" : "");
201
202 ✗ ImGui::SetNextItemWidth(configWidth + ImGui::GetStyle().ItemSpacing.x);
203 ✗ if (auto qCalculationAlgorithm = static_cast<int>(_qCalculationAlgorithm);
204 ✗ ImGui::Combo(fmt::format("Q calculation algorithm##{}", size_t(id)).c_str(), &qCalculationAlgorithm, "Van Loan\0Taylor 1st Order (Groves 2013)\0\0"))
205 {
206 ✗ _qCalculationAlgorithm = static_cast<decltype(_qCalculationAlgorithm)>(qCalculationAlgorithm);
207 ✗ LOG_DEBUG("{}: Q calculation algorithm changed to {}", nameId(), fmt::underlying(_qCalculationAlgorithm));
208 ✗ flow::ApplyChanges();
209 }
210
211 ✗ ImGui::Separator();
212
213 // ###########################################################################################################
214 // Q - System/Process noise covariance matrix
215 // ###########################################################################################################
216
217 ✗ ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
218 ✗ if (ImGui::TreeNode(fmt::format("Q - System/Process noise covariance matrix##{}", size_t(id)).c_str()))
219 {
220 // --------------------------------------------- Accelerometer -----------------------------------------------
221 ✗ if (_qCalculationAlgorithm == QCalculationAlgorithm::VanLoan)
222 {
223 ✗ ImGui::SetNextItemWidth(configWidth + ImGui::GetStyle().ItemSpacing.x);
224 ✗ if (auto randomProcessAccel = static_cast<int>(_randomProcessAccel);
225 ✗ ImGui::Combo(fmt::format("Random Process Accelerometer##{}", size_t(id)).c_str(), &randomProcessAccel, "Random Walk\0"
226 "Gauss-Markov 1st Order\0\0"))
227 {
228 ✗ _randomProcessAccel = static_cast<decltype(_randomProcessAccel)>(randomProcessAccel);
229 ✗ LOG_DEBUG("{}: randomProcessAccel changed to {}", nameId(), fmt::underlying(_randomProcessAccel));
230 ✗ flow::ApplyChanges();
231 }
232 }
233
234 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Standard deviation of the noise on the\naccelerometer specific-force measurements##{}", size_t(id)).c_str(),
235 ✗ configWidth, unitWidth, _stdev_ra.data(), _stdevAccelNoiseUnits, "mg/√(Hz)\0m/s^2/√(Hz)\0\0",
236 "%.2e", ImGuiInputTextFlags_CharsScientific))
237 {
238 ✗ LOG_DEBUG("{}: stdev_ra changed to {}", nameId(), _stdev_ra.transpose());
239 ✗ LOG_DEBUG("{}: stdevAccelNoiseUnits changed to {}", nameId(), fmt::underlying(_stdevAccelNoiseUnits));
240 ✗ flow::ApplyChanges();
241 }
242 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Standard deviation σ of the accel {}##{}",
243 ✗ _qCalculationAlgorithm == QCalculationAlgorithm::Taylor1
244 ✗ ? "dynamic bias, in σ²/τ"
245 ✗ : (_randomProcessAccel == RandomProcess::RandomWalk ? "bias noise" : "bias noise, in √(2σ²β)"),
246 ✗ size_t(id))
247 .c_str(),
248 ✗ configWidth, unitWidth, _stdev_bad.data(), _stdevAccelBiasUnits, "µg\0m/s^2\0\0",
249 "%.2e", ImGuiInputTextFlags_CharsScientific))
250 {
251 ✗ LOG_DEBUG("{}: stdev_bad changed to {}", nameId(), _stdev_bad.transpose());
252 ✗ LOG_DEBUG("{}: stdevAccelBiasUnits changed to {}", nameId(), fmt::underlying(_stdevAccelBiasUnits));
253 ✗ flow::ApplyChanges();
254 }
255
256 ✗ if (_randomProcessAccel == RandomProcess::GaussMarkov1 || _qCalculationAlgorithm == QCalculationAlgorithm::Taylor1)
257 {
258 ✗ int unitCorrelationLength = 0;
259 ✗ if (gui::widgets::InputDouble3LWithUnit(fmt::format("Correlation length τ of the accel {}##Correlation length τ of the accel dynamic bias {}",
260 ✗ _qCalculationAlgorithm == QCalculationAlgorithm::VanLoan ? "bias noise" : "dynamic bias", size_t(id))
261 .c_str(),
262 configWidth, unitWidth, _tau_bad.data(), 0., std::numeric_limits<double>::max(),
263 unitCorrelationLength, "s\0\0", "%.2e", ImGuiInputTextFlags_CharsScientific))
264 {
265 ✗ LOG_DEBUG("{}: tau_bad changed to {}", nameId(), _tau_bad);
266 ✗ flow::ApplyChanges();
267 }
268 }
269
270 ✗ ImGui::SetCursorPosY(ImGui::GetCursorPosY() + 20.F);
271
272 // ----------------------------------------------- Gyroscope -------------------------------------------------
273
274 ✗ if (_qCalculationAlgorithm == QCalculationAlgorithm::VanLoan)
275 {
276 ✗ ImGui::SetNextItemWidth(configWidth + ImGui::GetStyle().ItemSpacing.x);
277 ✗ if (auto randomProcessGyro = static_cast<int>(_randomProcessGyro);
278 ✗ ImGui::Combo(fmt::format("Random Process Gyroscope##{}", size_t(id)).c_str(), &randomProcessGyro, "Random Walk\0"
279 "Gauss-Markov 1st Order\0\0"))
280 {
281 ✗ _randomProcessGyro = static_cast<decltype(_randomProcessGyro)>(randomProcessGyro);
282 ✗ LOG_DEBUG("{}: randomProcessGyro changed to {}", nameId(), fmt::underlying(_randomProcessGyro));
283 ✗ flow::ApplyChanges();
284 }
285 }
286
287 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Standard deviation of the noise on\nthe gyro angular-rate measurements##{}", size_t(id)).c_str(),
288 ✗ configWidth, unitWidth, _stdev_rg.data(), _stdevGyroNoiseUnits, "deg/hr/√(Hz)\0rad/s/√(Hz)\0\0",
289 "%.2e", ImGuiInputTextFlags_CharsScientific))
290 {
291 ✗ LOG_DEBUG("{}: stdev_rg changed to {}", nameId(), _stdev_rg.transpose());
292 ✗ LOG_DEBUG("{}: stdevGyroNoiseUnits changed to {}", nameId(), fmt::underlying(_stdevGyroNoiseUnits));
293 ✗ flow::ApplyChanges();
294 }
295 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Standard deviation σ of the gyro {}##{}",
296 ✗ _qCalculationAlgorithm == QCalculationAlgorithm::Taylor1
297 ✗ ? "dynamic bias, in σ²/τ"
298 ✗ : (_randomProcessGyro == RandomProcess::RandomWalk ? "bias noise" : "bias noise, in √(2σ²β)"),
299 ✗ size_t(id))
300 .c_str(),
301 ✗ configWidth, unitWidth, _stdev_bgd.data(), _stdevGyroBiasUnits, "°/h\0rad/s\0\0",
302 "%.2e", ImGuiInputTextFlags_CharsScientific))
303 {
304 ✗ LOG_DEBUG("{}: stdev_bgd changed to {}", nameId(), _stdev_bgd.transpose());
305 ✗ LOG_DEBUG("{}: stdevGyroBiasUnits changed to {}", nameId(), fmt::underlying(_stdevGyroBiasUnits));
306 ✗ flow::ApplyChanges();
307 }
308
309 ✗ if (_randomProcessGyro == RandomProcess::GaussMarkov1 || _qCalculationAlgorithm == QCalculationAlgorithm::Taylor1)
310 {
311 ✗ int unitCorrelationLength = 0;
312 ✗ if (gui::widgets::InputDouble3LWithUnit(fmt::format("Correlation length τ of the gyro {}##Correlation length τ of the gyro dynamic bias {}",
313 ✗ _qCalculationAlgorithm == QCalculationAlgorithm::VanLoan ? "bias noise" : "dynamic bias", size_t(id))
314 .c_str(),
315 configWidth, unitWidth, _tau_bgd.data(), 0., std::numeric_limits<double>::max(),
316 unitCorrelationLength, "s\0\0", "%.2e", ImGuiInputTextFlags_CharsScientific))
317 {
318 ✗ LOG_DEBUG("{}: tau_bgd changed to {}", nameId(), _tau_bgd);
319 ✗ flow::ApplyChanges();
320 }
321 }
322
323 ✗ ImGui::TreePop();
324 }
325
326 // ###########################################################################################################
327 // Measurement Uncertainties 𝐑
328 // ###########################################################################################################
329
330 ✗ ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
331 ✗ if (ImGui::TreeNode(fmt::format("R - Measurement noise covariance matrix##{}", size_t(id)).c_str()))
332 {
333 ✗ float curPosX = ImGui::GetCursorPosX();
334 ✗ if (ImGui::Checkbox(fmt::format("##Override R Position {}", size_t(id)).c_str(), &_gnssMeasurementUncertaintyPositionOverride))
335 {
336 ✗ flow::ApplyChanges();
337 }
338 ✗ if (ImGui::IsItemHovered()) { ImGui::SetTooltip("Override the position variance in the measurements with this values."); }
339 ✗ ImGui::SameLine();
340 ✗ float checkWidth = ImGui::GetCursorPosX() - curPosX;
341 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("{} of the GNSS position measurements##{}",
342 ✗ _gnssMeasurementUncertaintyPositionUnit == GnssMeasurementUncertaintyPositionUnit::rad2_rad2_m2
343 ✗ || _gnssMeasurementUncertaintyPositionUnit == GnssMeasurementUncertaintyPositionUnit::meter2
344 ✗ ? "Variance"
345 : "Standard deviation",
346 ✗ size_t(id))
347 .c_str(),
348 ✗ configWidth - checkWidth, unitWidth, _gnssMeasurementUncertaintyPosition.data(), _gnssMeasurementUncertaintyPositionUnit, "rad^2, rad^2, m^2\0"
349 "rad, rad, m\0"
350 "m^2, m^2, m^2\0"
351 "m, m, m\0\0",
352 "%.2e", ImGuiInputTextFlags_CharsScientific))
353 {
354 ✗ LOG_DEBUG("{}: gnssMeasurementUncertaintyPosition changed to {}", nameId(), _gnssMeasurementUncertaintyPosition.transpose());
355 ✗ LOG_DEBUG("{}: gnssMeasurementUncertaintyPositionUnit changed to {}", nameId(), fmt::underlying(_gnssMeasurementUncertaintyPositionUnit));
356 ✗ flow::ApplyChanges();
357 }
358
359 ✗ if (ImGui::Checkbox(fmt::format("##Override R Velocity {}", size_t(id)).c_str(), &_gnssMeasurementUncertaintyVelocityOverride))
360 {
361 ✗ flow::ApplyChanges();
362 }
363 ✗ if (ImGui::IsItemHovered()) { ImGui::SetTooltip("Override the velocity variance in the measurements with this values."); }
364 ✗ ImGui::SameLine();
365 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("{} of the GNSS velocity measurements##{}", _gnssMeasurementUncertaintyVelocityUnit == GnssMeasurementUncertaintyVelocityUnit::m2_s2 ? "Variance" : "Standard deviation",
366 ✗ size_t(id))
367 .c_str(),
368 ✗ configWidth - checkWidth, unitWidth, _gnssMeasurementUncertaintyVelocity.data(), _gnssMeasurementUncertaintyVelocityUnit, "m^2/s^2\0"
369 "m/s\0\0",
370 "%.2e", ImGuiInputTextFlags_CharsScientific))
371 {
372 ✗ LOG_DEBUG("{}: gnssMeasurementUncertaintyVelocity changed to {}", nameId(), _gnssMeasurementUncertaintyVelocity);
373 ✗ LOG_DEBUG("{}: gnssMeasurementUncertaintyVelocityUnit changed to {}", nameId(), fmt::underlying(_gnssMeasurementUncertaintyVelocityUnit));
374 ✗ flow::ApplyChanges();
375 }
376
377 ✗ ImGui::TreePop();
378 }
379
380 // ###########################################################################################################
381 // 𝐏 Error covariance matrix
382 // ###########################################################################################################
383
384 ✗ ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
385 ✗ if (ImGui::TreeNode(fmt::format("P Error covariance matrix (init)##{}", size_t(id)).c_str()))
386 {
387 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Position covariance ({})##{}",
388 ✗ _initCovariancePositionUnit == InitCovariancePositionUnit::rad2_rad2_m2
389 ✗ || _initCovariancePositionUnit == InitCovariancePositionUnit::meter2
390 ✗ ? "Variance σ²"
391 : "Standard deviation σ",
392 ✗ size_t(id))
393 .c_str(),
394 ✗ configWidth, unitWidth, _initCovariancePosition.data(), _initCovariancePositionUnit, "rad^2, rad^2, m^2\0"
395 "rad, rad, m\0"
396 "m^2, m^2, m^2\0"
397 "m, m, m\0\0",
398 "%.2e", ImGuiInputTextFlags_CharsScientific))
399 {
400 ✗ LOG_DEBUG("{}: initCovariancePosition changed to {}", nameId(), _initCovariancePosition);
401 ✗ LOG_DEBUG("{}: initCovariancePositionUnit changed to {}", nameId(), fmt::underlying(_initCovariancePositionUnit));
402 ✗ flow::ApplyChanges();
403 }
404
405 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Velocity covariance ({})##{}",
406 ✗ _initCovarianceVelocityUnit == InitCovarianceVelocityUnit::m2_s2
407 ✗ ? "Variance σ²"
408 : "Standard deviation σ",
409 ✗ size_t(id))
410 .c_str(),
411 ✗ configWidth, unitWidth, _initCovarianceVelocity.data(), _initCovarianceVelocityUnit, "m^2/s^2\0"
412 "m/s\0\0",
413 "%.2e", ImGuiInputTextFlags_CharsScientific))
414 {
415 ✗ LOG_DEBUG("{}: initCovarianceVelocity changed to {}", nameId(), _initCovarianceVelocity);
416 ✗ LOG_DEBUG("{}: initCovarianceVelocityUnit changed to {}", nameId(), fmt::underlying(_initCovarianceVelocityUnit));
417 ✗ flow::ApplyChanges();
418 }
419
420 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Flight Angles covariance ({})##{}",
421 ✗ _initCovarianceAttitudeAnglesUnit == InitCovarianceAttitudeAnglesUnit::rad2
422 ✗ || _initCovarianceAttitudeAnglesUnit == InitCovarianceAttitudeAnglesUnit::deg2
423 ✗ ? "Variance σ²"
424 : "Standard deviation σ",
425 ✗ size_t(id))
426 .c_str(),
427 ✗ configWidth, unitWidth, _initCovarianceAttitudeAngles.data(), _initCovarianceAttitudeAnglesUnit, "rad^2\0"
428 "deg^2\0"
429 "rad\0"
430 "deg\0\0",
431 "%.2e", ImGuiInputTextFlags_CharsScientific))
432 {
433 ✗ LOG_DEBUG("{}: initCovarianceAttitudeAngles changed to {}", nameId(), _initCovarianceAttitudeAngles);
434 ✗ LOG_DEBUG("{}: initCovarianceAttitudeAnglesUnit changed to {}", nameId(), fmt::underlying(_initCovarianceAttitudeAnglesUnit));
435 ✗ flow::ApplyChanges();
436 }
437 ✗ ImGui::SameLine();
438 ✗ gui::widgets::HelpMarker(_inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::ECEF
439 ? "Angles rotating the ECEF frame into the body frame."
440 : "Angles rotating the local navigation frame into the body frame (Roll, Pitch, Yaw)");
441
442 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Accelerometer Bias covariance ({})##{}",
443 ✗ _initCovarianceBiasAccelUnit == InitCovarianceBiasAccelUnit::m2_s4
444 ✗ ? "Variance σ²"
445 : "Standard deviation σ",
446 ✗ size_t(id))
447 .c_str(),
448 ✗ configWidth, unitWidth, _initCovarianceBiasAccel.data(), _initCovarianceBiasAccelUnit, "m^2/s^4\0"
449 "m/s^2\0\0",
450 "%.2e", ImGuiInputTextFlags_CharsScientific))
451 {
452 ✗ LOG_DEBUG("{}: initCovarianceBiasAccel changed to {}", nameId(), _initCovarianceBiasAccel);
453 ✗ LOG_DEBUG("{}: initCovarianceBiasAccelUnit changed to {}", nameId(), fmt::underlying(_initCovarianceBiasAccelUnit));
454 ✗ flow::ApplyChanges();
455 }
456
457 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Gyroscope Bias covariance ({})##{}",
458 ✗ _initCovarianceBiasGyroUnit == InitCovarianceBiasGyroUnit::rad2_s2
459 ✗ || _initCovarianceBiasGyroUnit == InitCovarianceBiasGyroUnit::deg2_s2
460 ✗ ? "Variance σ²"
461 : "Standard deviation σ",
462 ✗ size_t(id))
463 .c_str(),
464 ✗ configWidth, unitWidth, _initCovarianceBiasGyro.data(), _initCovarianceBiasGyroUnit, "rad^2/s^2\0"
465 "deg^2/s^2\0"
466 "rad/s\0"
467 "deg/s\0\0",
468 "%.2e", ImGuiInputTextFlags_CharsScientific))
469 {
470 ✗ LOG_DEBUG("{}: initCovarianceBiasGyro changed to {}", nameId(), _initCovarianceBiasGyro);
471 ✗ LOG_DEBUG("{}: initCovarianceBiasGyroUnit changed to {}", nameId(), fmt::underlying(_initCovarianceBiasGyroUnit));
472 ✗ flow::ApplyChanges();
473 }
474
475 ✗ ImGui::TreePop();
476 }
477
478 // ###########################################################################################################
479 // IMU biases
480 // ###########################################################################################################
481
482 ✗ ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
483 ✗ if (ImGui::TreeNode(fmt::format("IMU biases (init)##{}", size_t(id)).c_str()))
484 {
485 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Accelerometer biases##{}", size_t(id)).c_str(),
486 ✗ configWidth, unitWidth, _initBiasAccel.data(), _initBiasAccelUnit, "m/s^2\0\0",
487 "%.2e", ImGuiInputTextFlags_CharsScientific))
488 {
489 ✗ LOG_DEBUG("{}: initBiasAccel changed to {}", nameId(), _initBiasAccel.transpose());
490 ✗ LOG_DEBUG("{}: initBiasAccelUnit changed to {}", nameId(), fmt::underlying(_initBiasAccelUnit));
491 ✗ flow::ApplyChanges();
492 }
493 ✗ ImGui::SameLine();
494 ✗ gui::widgets::HelpMarker("In body frame coordinates");
495 ✗ if (gui::widgets::InputDouble3WithUnit(fmt::format("Gyro biases##{}", size_t(id)).c_str(),
496 ✗ configWidth, unitWidth, _initBiasGyro.data(), _initBiasGyroUnit, "rad/s\0deg/s\0\0",
497 "%.2e", ImGuiInputTextFlags_CharsScientific))
498 {
499 ✗ LOG_DEBUG("{}: initBiasGyro changed to {}", nameId(), _initBiasGyro.transpose());
500 ✗ LOG_DEBUG("{}: initBiasGyroUnit changed to {}", nameId(), fmt::underlying(_initBiasGyroUnit));
501 ✗ flow::ApplyChanges();
502 }
503 ✗ ImGui::SameLine();
504 ✗ gui::widgets::HelpMarker("In body frame coordinates");
505
506 ✗ ImGui::TreePop();
507 }
508
509 ✗ ImGui::SetNextItemOpen(false, ImGuiCond_FirstUseEver);
510 ✗ if (ImGui::TreeNode(fmt::format("Kalman Filter matrices##{}", size_t(id)).c_str()))
511 {
512 ✗ _kalmanFilter.showKalmanFilterMatrixViews(std::to_string(size_t(id)).c_str());
513 ✗ ImGui::TreePop();
514 }
515
516 ✗ ImGui::Separator();
517
518 ✗ if (ImGui::Checkbox(fmt::format("Rank check for Kalman filter matrices##{}", size_t(id)).c_str(), &_checkKalmanMatricesRanks))
519 {
520 ✗ LOG_DEBUG("{}: checkKalmanMatricesRanks {}", nameId(), _checkKalmanMatricesRanks);
521 ✗ flow::ApplyChanges();
522 }
523 }
524 ✗ }
525
526 ✗ [[nodiscard]] json NAV::LooselyCoupledKF::save() const
527 {
528 LOG_TRACE("{}: called", nameId());
529
530 ✗ json j;
531
532 ✗ j["inertialIntegrator"] = _inertialIntegrator;
533 ✗ j["preferAccelerationOverDeltaMeasurements"] = _preferAccelerationOverDeltaMeasurements;
534 ✗ j["initalRollPitchYaw"] = _initalRollPitchYaw;
535 ✗ j["initializeStateOverExternalPin"] = _initializeStateOverExternalPin;
536
537 ✗ j["checkKalmanMatricesRanks"] = _checkKalmanMatricesRanks;
538
539 ✗ j["phiCalculationAlgorithm"] = _phiCalculationAlgorithm;
540 ✗ j["phiCalculationTaylorOrder"] = _phiCalculationTaylorOrder;
541 ✗ j["qCalculationAlgorithm"] = _qCalculationAlgorithm;
542
543 ✗ j["randomProcessAccel"] = _randomProcessAccel;
544 ✗ j["randomProcessGyro"] = _randomProcessGyro;
545 ✗ j["stdev_ra"] = _stdev_ra;
546 ✗ j["stdevAccelNoiseUnits"] = _stdevAccelNoiseUnits;
547 ✗ j["stdev_rg"] = _stdev_rg;
548 ✗ j["stdevGyroNoiseUnits"] = _stdevGyroNoiseUnits;
549 ✗ j["stdev_bad"] = _stdev_bad;
550 ✗ j["tau_bad"] = _tau_bad;
551 ✗ j["stdevAccelBiasUnits"] = _stdevAccelBiasUnits;
552 ✗ j["stdev_bgd"] = _stdev_bgd;
553 ✗ j["tau_bgd"] = _tau_bgd;
554 ✗ j["stdevGyroBiasUnits"] = _stdevGyroBiasUnits;
555
556 ✗ j["gnssMeasurementUncertaintyPositionUnit"] = _gnssMeasurementUncertaintyPositionUnit;
557 ✗ j["gnssMeasurementUncertaintyPosition"] = _gnssMeasurementUncertaintyPosition;
558 ✗ j["gnssMeasurementUncertaintyPositionOverride"] = _gnssMeasurementUncertaintyPositionOverride;
559 ✗ j["gnssMeasurementUncertaintyVelocityUnit"] = _gnssMeasurementUncertaintyVelocityUnit;
560 ✗ j["gnssMeasurementUncertaintyVelocity"] = _gnssMeasurementUncertaintyVelocity;
561 ✗ j["gnssMeasurementUncertaintyVelocityOverride"] = _gnssMeasurementUncertaintyVelocityOverride;
562
563 ✗ j["initCovariancePositionUnit"] = _initCovariancePositionUnit;
564 ✗ j["initCovariancePosition"] = _initCovariancePosition;
565 ✗ j["initCovarianceVelocityUnit"] = _initCovarianceVelocityUnit;
566 ✗ j["initCovarianceVelocity"] = _initCovarianceVelocity;
567 ✗ j["initCovarianceAttitudeAnglesUnit"] = _initCovarianceAttitudeAnglesUnit;
568 ✗ j["initCovarianceAttitudeAngles"] = _initCovarianceAttitudeAngles;
569 ✗ j["initCovarianceBiasAccelUnit"] = _initCovarianceBiasAccelUnit;
570 ✗ j["initCovarianceBiasAccel"] = _initCovarianceBiasAccel;
571 ✗ j["initCovarianceBiasGyroUnit"] = _initCovarianceBiasGyroUnit;
572 ✗ j["initCovarianceBiasGyro"] = _initCovarianceBiasGyro;
573
574 ✗ j["initBiasAccel"] = _initBiasAccel;
575 ✗ j["initBiasAccelUnit"] = _initBiasAccelUnit;
576 ✗ j["initBiasGyro"] = _initBiasGyro;
577 ✗ j["initBiasGyroUnit"] = _initBiasGyroUnit;
578
579 ✗ return j;
580 ✗ }
581
582 17 void NAV::LooselyCoupledKF::restore(json const& j)
583 {
584 LOG_TRACE("{}: called", nameId());
585
586
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("inertialIntegrator"))
587 {
588 17 j.at("inertialIntegrator").get_to(_inertialIntegrator);
589 }
590
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("preferAccelerationOverDeltaMeasurements"))
591 {
592 17 j.at("preferAccelerationOverDeltaMeasurements").get_to(_preferAccelerationOverDeltaMeasurements);
593 }
594
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initalRollPitchYaw"))
595 {
596 17 j.at("initalRollPitchYaw").get_to(_initalRollPitchYaw);
597 }
598
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initializeStateOverExternalPin"))
599 {
600 17 j.at("initializeStateOverExternalPin").get_to(_initializeStateOverExternalPin);
601 17 updateExternalPvaInitPin();
602 }
603
604
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("checkKalmanMatricesRanks"))
605 {
606 17 j.at("checkKalmanMatricesRanks").get_to(_checkKalmanMatricesRanks);
607 }
608
609
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("phiCalculationAlgorithm"))
610 {
611 17 j.at("phiCalculationAlgorithm").get_to(_phiCalculationAlgorithm);
612 }
613
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("phiCalculationTaylorOrder"))
614 {
615 17 j.at("phiCalculationTaylorOrder").get_to(_phiCalculationTaylorOrder);
616 }
617
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("qCalculationAlgorithm"))
618 {
619 17 j.at("qCalculationAlgorithm").get_to(_qCalculationAlgorithm);
620 }
621 // ------------------------------- 𝐐 System/Process noise covariance matrix ---------------------------------
622
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("randomProcessAccel"))
623 {
624 17 j.at("randomProcessAccel").get_to(_randomProcessAccel);
625 }
626
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("randomProcessGyro"))
627 {
628 17 j.at("randomProcessGyro").get_to(_randomProcessGyro);
629 }
630
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("stdev_ra"))
631 {
632
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");
633 }
634
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("stdevAccelNoiseUnits"))
635 {
636 17 j.at("stdevAccelNoiseUnits").get_to(_stdevAccelNoiseUnits);
637 }
638
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("stdev_rg"))
639 {
640
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");
641 }
642
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("stdevGyroNoiseUnits"))
643 {
644 17 j.at("stdevGyroNoiseUnits").get_to(_stdevGyroNoiseUnits);
645 }
646
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("stdev_bad"))
647 {
648
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");
649 }
650
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("tau_bad"))
651 {
652
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");
653 }
654
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("stdevAccelBiasUnits"))
655 {
656 17 j.at("stdevAccelBiasUnits").get_to(_stdevAccelBiasUnits);
657 }
658
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("stdev_bgd"))
659 {
660
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");
661 }
662
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("tau_bgd"))
663 {
664
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");
665 }
666
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("stdevGyroBiasUnits"))
667 {
668 17 j.at("stdevGyroBiasUnits").get_to(_stdevGyroBiasUnits);
669 }
670 // -------------------------------- 𝐑 Measurement noise covariance matrix -----------------------------------
671
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("gnssMeasurementUncertaintyPositionUnit"))
672 {
673 17 j.at("gnssMeasurementUncertaintyPositionUnit").get_to(_gnssMeasurementUncertaintyPositionUnit);
674 }
675
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("gnssMeasurementUncertaintyPosition"))
676 {
677
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");
678 }
679
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 16 times.
 17 if (j.contains("gnssMeasurementUncertaintyPositionOverride"))
680 {
681 1 j.at("gnssMeasurementUncertaintyPositionOverride").get_to(_gnssMeasurementUncertaintyPositionOverride);
682 }
683
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("gnssMeasurementUncertaintyVelocityUnit"))
684 {
685 17 j.at("gnssMeasurementUncertaintyVelocityUnit").get_to(_gnssMeasurementUncertaintyVelocityUnit);
686 }
687
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("gnssMeasurementUncertaintyVelocity"))
688 {
689
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");
690 }
691
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 16 times.
 17 if (j.contains("gnssMeasurementUncertaintyVelocityOverride"))
692 {
693 1 j.at("gnssMeasurementUncertaintyVelocityOverride").get_to(_gnssMeasurementUncertaintyVelocityOverride);
694 }
695 // -------------------------------------- 𝐏 Error covariance matrix -----------------------------------------
696
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initCovariancePositionUnit"))
697 {
698 17 j.at("initCovariancePositionUnit").get_to(_initCovariancePositionUnit);
699 }
700
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initCovariancePosition"))
701 {
702
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");
703 }
704
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initCovarianceVelocityUnit"))
705 {
706 17 j.at("initCovarianceVelocityUnit").get_to(_initCovarianceVelocityUnit);
707 }
708
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initCovarianceVelocity"))
709 {
710
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");
711 }
712
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initCovarianceAttitudeAnglesUnit"))
713 {
714 17 j.at("initCovarianceAttitudeAnglesUnit").get_to(_initCovarianceAttitudeAnglesUnit);
715 }
716
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initCovarianceAttitudeAngles"))
717 {
718
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");
719 }
720
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initCovarianceBiasAccelUnit"))
721 {
722 17 j.at("initCovarianceBiasAccelUnit").get_to(_initCovarianceBiasAccelUnit);
723 }
724
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initCovarianceBiasAccel"))
725 {
726
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");
727 }
728
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initCovarianceBiasGyroUnit"))
729 {
730 17 j.at("initCovarianceBiasGyroUnit").get_to(_initCovarianceBiasGyroUnit);
731 }
732
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initCovarianceBiasGyro"))
733 {
734
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");
735 }
736
737 // ---------------------------------------- Initial IMU biases -------------------------------------------
738
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initBiasAccel"))
739 {
740
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");
741 }
742
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initBiasAccelUnit"))
743 {
744 17 _initBiasAccelUnit = j.at("initBiasAccelUnit");
745 }
746
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initBiasGyro"))
747 {
748
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");
749 }
750
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (j.contains("initBiasGyroUnit"))
751 {
752 17 _initBiasGyroUnit = j.at("initBiasGyroUnit");
753 }
754 17 }
755
756 51 bool NAV::LooselyCoupledKF::initialize()
757 {
758 LOG_TRACE("{}: called", nameId());
759
760 51 inputPins[INPUT_PORT_INDEX_GNSS].priority = 2; // PosVel used for initialization
761
762
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 _inertialIntegrator.reset();
763 51 _lastImuObs = nullptr;
764 51 _externalInitTime.reset();
765 51 _initialSensorBiasesApplied = false;
766
767
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 _kalmanFilter.setZero();
768
769 // Initial Covariance of the attitude angles in [rad²]
770
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();
771
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 51 times.
 51 if (_initCovarianceAttitudeAnglesUnit == InitCovarianceAttitudeAnglesUnit::rad2)
772 {
773 ✗ variance_angles = _initCovarianceAttitudeAngles;
774 }
775
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 51 times.
 51 else if (_initCovarianceAttitudeAnglesUnit == InitCovarianceAttitudeAnglesUnit::deg2)
776 {
777 ✗ variance_angles = deg2rad(_initCovarianceAttitudeAngles);
778 }
779
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 51 times.
 51 else if (_initCovarianceAttitudeAnglesUnit == InitCovarianceAttitudeAnglesUnit::rad)
780 {
781 ✗ variance_angles = _initCovarianceAttitudeAngles.array().pow(2);
782 }
783
1/2
✓ Branch 0 taken 51 times.
✗ Branch 1 not taken.
 51 else if (_initCovarianceAttitudeAnglesUnit == InitCovarianceAttitudeAnglesUnit::deg)
784 {
785
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);
786 }
787
788 // Initial Covariance of the velocity in [m²/s²]
789
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();
790
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 51 times.
 51 if (_initCovarianceVelocityUnit == InitCovarianceVelocityUnit::m2_s2)
791 {
792 ✗ variance_vel = _initCovarianceVelocity;
793 }
794
1/2
✓ Branch 0 taken 51 times.
✗ Branch 1 not taken.
 51 else if (_initCovarianceVelocityUnit == InitCovarianceVelocityUnit::m_s)
795 {
796
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);
797 }
798
799 // Initial Covariance of the position in [m²]
800
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();
801 // Initial Covariance of the position in [rad² rad² m²]
802
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();
803
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 51 times.
 51 if (_initCovariancePositionUnit == InitCovariancePositionUnit::rad2_rad2_m2)
804 {
805 ✗ e_variance = trafo::lla2ecef_WGS84(_initCovariancePosition.cwiseSqrt()).array().pow(2);
806 ✗ lla_variance = _initCovariancePosition;
807 }
808
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 51 times.
 51 else if (_initCovariancePositionUnit == InitCovariancePositionUnit::rad_rad_m)
809 {
810 ✗ e_variance = trafo::lla2ecef_WGS84(_initCovariancePosition).array().pow(2);
811 ✗ lla_variance = _initCovariancePosition.array().pow(2);
812 }
813
1/2
✓ Branch 0 taken 51 times.
✗ Branch 1 not taken.
 51 else if (_initCovariancePositionUnit == InitCovariancePositionUnit::meter)
814 {
815
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);
816
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);
817 }
818 ✗ else if (_initCovariancePositionUnit == InitCovariancePositionUnit::meter2)
819 {
820 ✗ e_variance = _initCovariancePosition;
821 ✗ lla_variance = (trafo::ecef2lla_WGS84(trafo::ned2ecef(_initCovariancePosition.cwiseSqrt(), Eigen::Vector3d{ 0, 0, 0 }))).array().pow(2);
822 }
823
824 // Initial Covariance of the accelerometer biases in [m^2/s^4]
825
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();
826
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 51 times.
 51 if (_initCovarianceBiasAccelUnit == InitCovarianceBiasAccelUnit::m2_s4)
827 {
828 ✗ variance_accelBias = _initCovarianceBiasAccel;
829 }
830
1/2
✓ Branch 0 taken 51 times.
✗ Branch 1 not taken.
 51 else if (_initCovarianceBiasAccelUnit == InitCovarianceBiasAccelUnit::m_s2)
831 {
832
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);
833 }
834
835 // Initial Covariance of the gyroscope biases in [rad^2/s^2]
836
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();
837
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 51 times.
 51 if (_initCovarianceBiasGyroUnit == InitCovarianceBiasGyroUnit::rad2_s2)
838 {
839 ✗ variance_gyroBias = _initCovarianceBiasGyro;
840 }
841
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 51 times.
 51 else if (_initCovarianceBiasGyroUnit == InitCovarianceBiasGyroUnit::deg2_s2)
842 {
843 ✗ variance_gyroBias = deg2rad(_initCovarianceBiasGyro.array().sqrt()).array().pow(2);
844 }
845
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 48 times.
 51 else if (_initCovarianceBiasGyroUnit == InitCovarianceBiasGyroUnit::rad_s)
846 {
847
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);
848 }
849
1/2
✓ Branch 0 taken 48 times.
✗ Branch 1 not taken.
 48 else if (_initCovarianceBiasGyroUnit == InitCovarianceBiasGyroUnit::deg_s)
850 {
851
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);
852 }
853
854 // 𝐏 Error covariance matrix
855
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 102 _kalmanFilter.P = initialErrorCovarianceMatrix_P0(variance_angles, // Flight Angles covariance
856 variance_vel, // Velocity covariance
857
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
858 ? lla_variance
859 : e_variance, // Position (Lat, Lon, Alt) / ECEF covariance
860 variance_accelBias, // Accelerometer Bias covariance
861 51 variance_gyroBias); // Gyroscope Bias covariance
862
863 // Initial acceleration bias in body frame coordinates in [m/s^2]
864
2/4
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
 51 Eigen::Vector3d accelBias = Eigen::Vector3d::Zero();
865
1/2
✓ Branch 0 taken 51 times.
✗ Branch 1 not taken.
 51 if (_initBiasAccelUnit == InitBiasAccelUnit::m_s2)
866 {
867
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 accelBias = _initBiasAccel;
868 }
869
870 // Initial angular rate bias in body frame coordinates in [rad/s]
871
2/4
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
 51 Eigen::Vector3d gyroBias = Eigen::Vector3d::Zero();
872
1/2
✓ Branch 0 taken 51 times.
✗ Branch 1 not taken.
 51 if (_initBiasGyroUnit == InitBiasGyroUnit::deg_s)
873 {
874
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 gyroBias = deg2rad(_initBiasGyro);
875 }
876 ✗ else if (_initBiasGyroUnit == InitBiasGyroUnit::rad_s)
877 {
878 ✗ gyroBias = _initBiasGyro;
879 }
880
881
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());
882 LOG_DATA("{}: P_0 =\n{}", nameId(), _kalmanFilter.P);
883
884 51 return true;
885 }
886
887 17 void NAV::LooselyCoupledKF::deinitialize()
888 {
889 LOG_TRACE("{}: called", nameId());
890 17 }
891
892 25793 void NAV::LooselyCoupledKF::invokeCallbackWithPosVelAtt(const PosVelAtt& posVelAtt)
893 {
894
1/2
✓ Branch 1 taken 25793 times.
✗ Branch 2 not taken.
 25793 auto lckfSolution = std::make_shared<InsGnssLCKFSolution>();
895 25793 lckfSolution->insTime = posVelAtt.insTime;
896
2/2
✓ Branch 2 taken 25776 times.
✓ Branch 3 taken 17 times.
 25793 if (posVelAtt.e_CovarianceMatrix())
897 {
898
2/4
✓ Branch 5 taken 25776 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 25776 times.
✗ Branch 10 not taken.
 51552 lckfSolution->setStateAndStdDev_e(posVelAtt.e_position(), posVelAtt.e_CovarianceMatrix()->get()(PosVel::States::Pos, PosVel::States::Pos),
899
1/2
✓ Branch 4 taken 25776 times.
✗ Branch 5 not taken.
 51552 posVelAtt.e_velocity(), posVelAtt.e_CovarianceMatrix()->get()(PosVel::States::Vel, PosVel::States::Vel),
900 25776 posVelAtt.e_Quat_b());
901
2/4
✓ Branch 5 taken 25776 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 25776 times.
✗ Branch 9 not taken.
 25776 lckfSolution->setPosVelCovarianceMatrix_e(posVelAtt.e_CovarianceMatrix()->get()(PosVel::States::PosVel, PosVel::States::PosVel));
902 }
903 else
904 {
905
1/2
✓ Branch 5 taken 17 times.
✗ Branch 6 not taken.
 17 lckfSolution->setState_e(posVelAtt.e_position(), posVelAtt.e_velocity(), posVelAtt.e_Quat_b());
906 }
907
908
1/2
✓ Branch 1 taken 25793 times.
✗ Branch 2 not taken.
 51586 lckfSolution->frame = _inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::NED
909
2/2
✓ Branch 0 taken 12897 times.
✓ Branch 1 taken 12896 times.
 25793 ? InsGnssLCKFSolution::Frame::NED
910 : InsGnssLCKFSolution::Frame::ECEF;
911
912
2/2
✓ Branch 1 taken 25776 times.
✓ Branch 2 taken 17 times.
 25793 if (_lastImuObs)
913 {
914
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 = _lastImuObs->imuPos.b_quatAccel_p() * -_inertialIntegrator.p_getLastAccelerationBias();
915
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 = _lastImuObs->imuPos.b_quatGyro_p() * -_inertialIntegrator.p_getLastAngularRateBias();
916 }
917
1/2
✓ Branch 2 taken 25793 times.
✗ Branch 3 not taken.
 25793 invokeCallbacks(OUTPUT_PORT_INDEX_SOLUTION, lckfSolution);
918 25793 }
919
920 49998 void NAV::LooselyCoupledKF::recvImuObservation(InputPin::NodeDataQueue& queue, size_t /* pinIdx */)
921 {
922
1/2
✓ Branch 1 taken 49998 times.
✗ Branch 2 not taken.
 49998 auto nodeData = queue.extract_front();
923
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 49998 times.
 49998 if (nodeData->insTime.empty())
924 {
925 ✗ LOG_ERROR("{}: Can't set new imuObs__t0 because the observation has no time tag (insTime)", nameId());
926 ✗ return;
927 }
928
929 49998 std::shared_ptr<NAV::PosVelAtt> inertialNavSol = nullptr;
930
931 49998 _lastImuObs = std::static_pointer_cast<const ImuObs>(nodeData);
932
933 99996 if (!_preferAccelerationOverDeltaMeasurements
934
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() }))
935 {
936 ✗ auto obs = std::static_pointer_cast<const ImuObsWDelta>(nodeData);
937 LOG_DATA("{}: [{}] recvImuObsWDelta", nameId(), obs->insTime.toYMDHMS(GPST));
938
939 // Initialize biases
940 ✗ if (!_initialSensorBiasesApplied)
941 {
942 ✗ _inertialIntegrator.setTotalSensorBiases(obs->imuPos.p_quatAccel_b() * -_initBiasAccel, obs->imuPos.p_quatGyro_b() * -_initBiasGyro);
943 ✗ _initialSensorBiasesApplied = true;
944 }
945
946 ✗ inertialNavSol = _inertialIntegrator.calcInertialSolutionDelta(obs->insTime, obs->dtime, obs->dvel, obs->dtheta, obs->imuPos);
947 ✗ }
948 else
949 {
950 49998 auto obs = std::static_pointer_cast<const ImuObs>(nodeData);
951 LOG_DATA("{}: [{}] recvImuObs", nameId(), obs->insTime.toYMDHMS(GPST));
952
953 // Initialize biases
954
2/2
✓ Branch 0 taken 17 times.
✓ Branch 1 taken 49981 times.
 49998 if (!_initialSensorBiasesApplied)
955 {
956
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_quatAccel_b() * -_initBiasAccel, obs->imuPos.p_quatGyro_b() * -_initBiasGyro);
957 17 _initialSensorBiasesApplied = true;
958 }
959
960
1/2
✓ Branch 5 taken 49998 times.
✗ Branch 6 not taken.
 49998 inertialNavSol = _inertialIntegrator.calcInertialSolution(obs->insTime, obs->p_acceleration, obs->p_angularRate, obs->imuPos);
961 49998 }
962
7/10
✓ Branch 1 taken 49998 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49998 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 49998 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 49997 times.
✓ Branch 10 taken 1 times.
✓ Branch 11 taken 49997 times.
✓ Branch 12 taken 1 times.
 49998 if (inertialNavSol && _inertialIntegrator.getMeasurements().back().dt > 1e-8)
963 {
964
3/6
✓ Branch 3 taken 49997 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 49997 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 49997 times.
✗ Branch 11 not taken.
 49997 looselyCoupledPrediction(inertialNavSol, _inertialIntegrator.getMeasurements().back().dt, std::static_pointer_cast<const ImuObs>(nodeData)->imuPos);
965
966
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)
967 {
968
2/4
✓ Branch 2 taken 37109 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 37109 times.
✗ Branch 8 not taken.
 74218 inertialNavSol->setStateAndStdDev_n(inertialNavSol->lla_position(), _kalmanFilter.P(KFPos, KFPos),
969
1/2
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
 74218 inertialNavSol->n_velocity(), _kalmanFilter.P(KFVel, KFVel),
970 37109 inertialNavSol->n_Quat_b());
971
2/4
✓ Branch 2 taken 37109 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 37109 times.
✗ Branch 6 not taken.
 37109 inertialNavSol->setPosVelCovarianceMatrix_n(_kalmanFilter.P(KFPosVel, KFPosVel));
972 }
973 else // if (_inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::ECEF)
974 {
975
2/4
✓ Branch 2 taken 12888 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 12888 times.
✗ Branch 8 not taken.
 25776 inertialNavSol->setStateAndStdDev_e(inertialNavSol->e_position(), _kalmanFilter.P(KFPos, KFPos),
976
1/2
✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
 25776 inertialNavSol->e_velocity(), _kalmanFilter.P(KFVel, KFVel),
977 12888 inertialNavSol->e_Quat_b());
978
2/4
✓ Branch 2 taken 12888 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12888 times.
✗ Branch 6 not taken.
 12888 inertialNavSol->setPosVelCovarianceMatrix_e(_kalmanFilter.P(KFPosVel, KFPosVel));
979 }
980
981 LOG_DATA("{}: e_position = {}", nameId(), inertialNavSol->e_position().transpose());
982 LOG_DATA("{}: e_velocity = {}", nameId(), inertialNavSol->e_velocity().transpose());
983 LOG_DATA("{}: rollPitchYaw = {}", nameId(), rad2deg(inertialNavSol->rollPitchYaw()).transpose());
984
1/2
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
 49997 if (const auto& q = inputPins.at(INPUT_PORT_INDEX_GNSS).queue;
985
5/6
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
✓ Branch 7 taken 25776 times.
✓ Branch 8 taken 24221 times.
✓ Branch 9 taken 25776 times.
✓ Branch 10 taken 24221 times.
 49997 q.empty() || q.front()->insTime != nodeData->insTime)
986 {
987 LOG_DATA("{}: [{}] Sending out predicted solution", nameId(), inertialNavSol->insTime.toYMDHMS(GPST));
988
1/2
✓ Branch 2 taken 25776 times.
✗ Branch 3 not taken.
 25776 invokeCallbackWithPosVelAtt(*inertialNavSol);
989 }
990 }
991 49998 }
992
993 26830 void NAV::LooselyCoupledKF::recvPosVelObservation(InputPin::NodeDataQueue& queue, size_t /* pinIdx */)
994 {
995
1/2
✓ Branch 1 taken 26830 times.
✗ Branch 2 not taken.
 26830 auto obs = std::static_pointer_cast<const PosVel>(queue.extract_front());
996 LOG_DATA("{}: [{}] recvPosVelObservation", nameId(), obs->insTime.toYMDHMS(GPST));
997
998
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())
999 {
1000 ✗ inputPins[INPUT_PORT_INDEX_GNSS].priority = 0; // IMU obs (prediction) should be evaluated before the PosVel obs (update)
1001
1002 ✗ PosVelAtt posVelAtt;
1003 ✗ posVelAtt.insTime = obs->insTime;
1004 ✗ posVelAtt.setState_n(obs->lla_position(), obs->n_velocity(),
1005 ✗ trafo::n_Quat_b(deg2rad(_initalRollPitchYaw[0]), deg2rad(_initalRollPitchYaw[1]), deg2rad(_initalRollPitchYaw[2])));
1006
1007 ✗ _inertialIntegrator.setInitialState(posVelAtt);
1008 LOG_DATA("{}: e_position = {}", nameId(), posVelAtt.e_position().transpose());
1009 LOG_DATA("{}: e_velocity = {}", nameId(), posVelAtt.e_velocity().transpose());
1010 LOG_DATA("{}: rollPitchYaw = {}", nameId(), rad2deg(posVelAtt.rollPitchYaw()).transpose());
1011
1012 LOG_DATA("{}: [{}] Sending out initial solution", nameId(), obs->insTime.toYMDHMS(GPST));
1013 ✗ invokeCallbackWithPosVelAtt(posVelAtt);
1014 ✗ return;
1015 ✗ }
1016
1017
2/2
✓ Branch 2 taken 17 times.
✓ Branch 3 taken 26813 times.
 26830 if (_externalInitTime == obs->insTime) { return; }
1018
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 26813 times.
 26813 if (!_lastImuObs)
1019 {
1020 ✗ PosVelAtt posVelAtt;
1021 ✗ posVelAtt.insTime = obs->insTime;
1022 ✗ posVelAtt.setState_n(obs->lla_position(), obs->n_velocity(), _inertialIntegrator.getLatestState()->get().n_Quat_b());
1023 ✗ _inertialIntegrator.setState(posVelAtt);
1024
1025 LOG_DATA("{}: [{}] Sending out received solution, as no IMU data yet", nameId(), obs->insTime.toYMDHMS(GPST));
1026 ✗ invokeCallbackWithPosVelAtt(posVelAtt);
1027 ✗ return;
1028 ✗ }
1029
1030
1/2
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
 26813 looselyCoupledUpdate(obs);
1031 26830 }
1032
1033 17 void NAV::LooselyCoupledKF::recvPosVelAttInit(InputPin::NodeDataQueue& queue, size_t /* pinIdx */)
1034 {
1035
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 auto posVelAtt = std::static_pointer_cast<const PosVelAtt>(queue.extract_front());
1036 17 inputPins[INPUT_PORT_INDEX_POS_VEL_ATT_INIT].queueBlocked = true;
1037 17 inputPins[INPUT_PORT_INDEX_POS_VEL_ATT_INIT].queue.clear();
1038
1039 LOG_DATA("{}: recvPosVelAttInit at time [{}]", nameId(), posVelAtt->insTime.toYMDHMS());
1040
1041 17 inputPins[INPUT_PORT_INDEX_GNSS].priority = 0; // IMU obs (prediction) should be evaluated before the PosVel obs (update)
1042 17 _externalInitTime = posVelAtt->insTime;
1043
1044
1/2
✓ Branch 2 taken 17 times.
✗ Branch 3 not taken.
 17 _inertialIntegrator.setInitialState(*posVelAtt);
1045 LOG_DATA("{}: e_position = {}", nameId(), posVelAtt->e_position().transpose());
1046 LOG_DATA("{}: e_velocity = {}", nameId(), posVelAtt->e_velocity().transpose());
1047 LOG_DATA("{}: rollPitchYaw = {}", nameId(), rad2deg(posVelAtt->rollPitchYaw()).transpose());
1048
1049
1/2
✓ Branch 2 taken 17 times.
✗ Branch 3 not taken.
 17 invokeCallbackWithPosVelAtt(*posVelAtt);
1050 17 }
1051
1052 // ###########################################################################################################
1053 // Kalman Filter
1054 // ###########################################################################################################
1055
1056 49997 void NAV::LooselyCoupledKF::looselyCoupledPrediction(const std::shared_ptr<const PosVelAtt>& inertialNavSol, double tau_i, const ImuPos& imuPos)
1057 {
1058 LOG_DATA("{}: [{}] Predicting", nameId(), inertialNavSol->insTime.toYMDHMS(GPST));
1059
1060 // ------------------------------------------- GUI Parameters ----------------------------------------------
1061
1062 // 𝜎_ra Standard deviation of the noise on the accelerometer specific-force state [m / (s^2 · √(s))]
1063
2/4
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49997 times.
✗ Branch 5 not taken.
 49997 Eigen::Vector3d sigma_ra = Eigen::Vector3d::Zero();
1064
2/3
✓ Branch 0 taken 25776 times.
✓ Branch 1 taken 24221 times.
✗ Branch 2 not taken.
 49997 switch (_stdevAccelNoiseUnits)
1065 {
1066 25776 case StdevAccelNoiseUnits::mg_sqrtHz: // [mg / √(Hz)]
1067
2/4
✓ Branch 1 taken 25776 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25776 times.
✗ Branch 5 not taken.
 25776 sigma_ra = _stdev_ra * 1e-3; // [g / √(Hz)]
1068
1/2
✓ Branch 1 taken 25776 times.
✗ Branch 2 not taken.
 25776 sigma_ra *= InsConst::G_NORM; // [m / (s^2 · √(Hz))] = [m / (s · √(s))]
1069 // sigma_ra /= 1.; // [m / (s^2 · √(s))]
1070 25776 break;
1071 24221 case StdevAccelNoiseUnits::m_s2_sqrtHz: // [m / (s^2 · √(Hz))] = [m / (s · √(s))]
1072
1/2
✓ Branch 1 taken 24221 times.
✗ Branch 2 not taken.
 24221 sigma_ra = _stdev_ra;
1073 // sigma_ra /= 1.; // [m / (s^2 · √(s))]
1074 24221 break;
1075 }
1076 LOG_DATA("{}: sigma_ra = {} [m / (s^2 · √(s))]", nameId(), sigma_ra.transpose());
1077
1078 // 𝜎_rg Standard deviation of the noise on the gyro angular-rate state [rad / (s · √(s))]
1079
2/4
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49997 times.
✗ Branch 5 not taken.
 49997 Eigen::Vector3d sigma_rg = Eigen::Vector3d::Zero();
1080
2/3
✓ Branch 0 taken 25776 times.
✓ Branch 1 taken 24221 times.
✗ Branch 2 not taken.
 49997 switch (_stdevGyroNoiseUnits)
1081 {
1082 25776 case StdevGyroNoiseUnits::deg_hr_sqrtHz: // [deg / hr / √(Hz)] (see Woodman (2007) Chp. 3.2.2 - eq. 7 with seconds instead of hours)
1083
1/2
✓ Branch 1 taken 25776 times.
✗ Branch 2 not taken.
 25776 sigma_rg = deg2rad(_stdev_rg); // [rad / hr / √(Hz)]
1084
1/2
✓ Branch 1 taken 25776 times.
✗ Branch 2 not taken.
 25776 sigma_rg /= 60.; // [rad / √(hr)]
1085
1/2
✓ Branch 1 taken 25776 times.
✗ Branch 2 not taken.
 25776 sigma_rg /= 60.; // [rad / √(s)]
1086 // sigma_rg /= 1.; // [rad / (s · √(s))]
1087 25776 break;
1088 24221 case StdevGyroNoiseUnits::rad_s_sqrtHz: // [rad / (s · √(Hz))] = [rad / √(s)]
1089
1/2
✓ Branch 1 taken 24221 times.
✗ Branch 2 not taken.
 24221 sigma_rg = _stdev_rg;
1090 // sigma_rg /= 1.; // [rad / (s · √(s))]
1091 24221 break;
1092 }
1093 LOG_DATA("{}: sigma_rg = {} [rad / (s · √(s))]", nameId(), sigma_rg.transpose());
1094
1095 // 𝜎_bad Standard deviation of the accelerometer dynamic bias [m / s^2]
1096
2/4
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49997 times.
✗ Branch 5 not taken.
 49997 Eigen::Vector3d sigma_bad = Eigen::Vector3d::Zero();
1097
2/3
✓ Branch 0 taken 25776 times.
✓ Branch 1 taken 24221 times.
✗ Branch 2 not taken.
 49997 switch (_stdevAccelBiasUnits)
1098 {
1099 25776 case StdevAccelBiasUnits::microg: // [µg]
1100
2/4
✓ Branch 1 taken 25776 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25776 times.
✗ Branch 5 not taken.
 25776 sigma_bad = _stdev_bad * 1e-6; // [g]
1101
1/2
✓ Branch 1 taken 25776 times.
✗ Branch 2 not taken.
 25776 sigma_bad *= InsConst::G_NORM; // [m / s^2]
1102 25776 break;
1103 24221 case StdevAccelBiasUnits::m_s2: // [m / s^2]
1104
1/2
✓ Branch 1 taken 24221 times.
✗ Branch 2 not taken.
 24221 sigma_bad = _stdev_bad;
1105 24221 break;
1106 }
1107 LOG_DATA("{}: sigma_bad = {} [m / s^2]", nameId(), sigma_bad.transpose());
1108
1109 // 𝜎_bgd Standard deviation of the gyro dynamic bias [rad / s]
1110
2/4
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49997 times.
✗ Branch 5 not taken.
 49997 Eigen::Vector3d sigma_bgd = Eigen::Vector3d::Zero();
1111
1/3
✓ Branch 0 taken 49997 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 49997 switch (_stdevGyroBiasUnits)
1112 {
1113 49997 case StdevGyroBiasUnits::deg_h: // [° / h]
1114
2/4
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49997 times.
✗ Branch 5 not taken.
 49997 sigma_bgd = _stdev_bgd / 3600.0; // [° / s]
1115
1/2
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
 49997 sigma_bgd = deg2rad(sigma_bgd); // [rad / s]
1116 49997 break;
1117 ✗ case StdevGyroBiasUnits::rad_s: // [rad / s]
1118 ✗ sigma_bgd = _stdev_bgd;
1119 ✗ break;
1120 }
1121 LOG_DATA("{}: sigma_bgd = {} [rad / s]", nameId(), sigma_bgd.transpose());
1122
1123 // ---------------------------------------------- Prediction -------------------------------------------------
1124
1125 // Latitude 𝜙, longitude λ and altitude (height above ground) in [rad, rad, m] at the time tₖ₋₁
1126 49997 const Eigen::Vector3d& lla_position = inertialNavSol->lla_position();
1127 LOG_DATA("{}: lla_position = {} [rad, rad, m]", nameId(), lla_position.transpose());
1128 // Prime vertical radius of curvature (East/West) [m]
1129
2/4
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49997 times.
✗ Branch 5 not taken.
 49997 double R_E = calcEarthRadius_E(lla_position(0));
1130 LOG_DATA("{}: R_E = {} [m]", nameId(), R_E);
1131 // Geocentric Radius in [m]
1132
2/4
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49997 times.
✗ Branch 5 not taken.
 49997 double r_eS_e = calcGeocentricRadius(lla_position(0), R_E);
1133 LOG_DATA("{}: r_eS_e = {} [m]", nameId(), r_eS_e);
1134
1135
1/2
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
 49997 auto p_acceleration = _inertialIntegrator.p_calcCurrentAcceleration();
1136 // Acceleration in [m/s^2], in body coordinates
1137 49997 Eigen::Vector3d b_acceleration = p_acceleration
1138
2/4
✓ Branch 0 taken 49997 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 49997 times.
✗ Branch 4 not taken.
 49997 ? imuPos.b_quatAccel_p() * p_acceleration.value()
1139
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();
1140 LOG_DATA("{}: b_acceleration = {} [m/s^2]", nameId(), b_acceleration.transpose());
1141
1142
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)
1143 {
1144 // n_velocity (tₖ₋₁) Velocity in [m/s], in navigation coordinates, at the time tₖ₋₁
1145 37109 const Eigen::Vector3d& n_velocity = inertialNavSol->n_velocity();
1146 LOG_DATA("{}: n_velocity = {} [m / s]", nameId(), n_velocity.transpose());
1147 // q (tₖ₋₁) Quaternion, from body to navigation coordinates, at the time tₖ₋₁
1148 37109 const Eigen::Quaterniond& n_Quat_b = inertialNavSol->n_Quat_b();
1149 LOG_DATA("{}: n_Quat_b --> Roll, Pitch, Yaw = {} [deg]", nameId(), deg2rad(trafo::quat2eulerZYX(n_Quat_b).transpose()));
1150
1151 // Meridian radius of curvature in [m]
1152
2/4
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.
 37109 double R_N = calcEarthRadius_N(lla_position(0));
1153 LOG_DATA("{}: R_N = {} [m]", nameId(), R_N);
1154
1155 // Conversion matrix between cartesian and curvilinear perturbations to the position
1156
1/2
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
 37109 Eigen::Matrix3d T_rn_p = conversionMatrixCartesianCurvilinear(lla_position, R_N, R_E);
1157 LOG_DATA("{}: T_rn_p =\n{}", nameId(), T_rn_p);
1158
1159 // Gravitation at surface level in [m/s^2]
1160
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();
1161
1162 // omega_in^n = omega_ie^n + omega_en^n
1163
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
1164
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);
1165 LOG_DATA("{}: n_omega_in = {} [rad/s]", nameId(), n_omega_in.transpose());
1166
1167 // System Matrix
1168
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);
1169 LOG_DATA("{}: F =\n{}", nameId(), _kalmanFilter.F);
1170
1171
2/2
✓ Branch 0 taken 6444 times.
✓ Branch 1 taken 30665 times.
 37109 if (_qCalculationAlgorithm == QCalculationAlgorithm::Taylor1)
1172 {
1173 // 2. Calculate the system noise covariance matrix Q_{k-1}
1174
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(),
1175
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(),
1176
1/2
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
 6444 _tau_bad, _tau_bgd,
1177
1/2
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
 6444 _kalmanFilter.F.block<3>(KFVel, KFAtt), T_rn_p,
1178
1/2
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
 19332 n_Quat_b.toRotationMatrix(), tau_i);
1179 }
1180 }
1181 else // if (_inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::ECEF)
1182 {
1183 // e_position (tₖ₋₁) Position in [m/s], in ECEF coordinates, at the time tₖ₋₁
1184 12888 const Eigen::Vector3d& e_position = inertialNavSol->e_position();
1185 LOG_DATA("{}: e_position = {} [m]", nameId(), e_position.transpose());
1186 // q (tₖ₋₁) Quaternion, from body to Earth coordinates, at the time tₖ₋₁
1187 12888 const Eigen::Quaterniond& e_Quat_b = inertialNavSol->e_Quat_b();
1188 LOG_DATA("{}: e_Quat_b = {}", nameId(), e_Quat_b);
1189
1190 // Gravitation in [m/s^2] in ECEF coordinates
1191
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);
1192
1193 // System Matrix
1194
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);
1195 LOG_DATA("{}: F =\n{}", nameId(), _kalmanFilter.F);
1196
1197
2/2
✓ Branch 0 taken 6444 times.
✓ Branch 1 taken 6444 times.
 12888 if (_qCalculationAlgorithm == QCalculationAlgorithm::Taylor1)
1198 {
1199 // 2. Calculate the system noise covariance matrix Q_{k-1}
1200
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(),
1201
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(),
1202
1/2
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
 6444 _tau_bad, _tau_bgd,
1203
1/2
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
 6444 _kalmanFilter.F.block<3>(KFVel, KFAtt),
1204
1/2
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
 19332 e_Quat_b.toRotationMatrix(), tau_i);
1205 }
1206 }
1207
1208
2/2
✓ Branch 0 taken 37109 times.
✓ Branch 1 taken 12888 times.
 49997 if (_qCalculationAlgorithm == QCalculationAlgorithm::VanLoan)
1209 {
1210 // Noise Input Matrix
1211
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
1212 30665 ? inertialNavSol->n_Quat_b()
1213 43553 : inertialNavSol->e_Quat_b());
1214 LOG_DATA("{}: G =\n{}", nameId(), _kalmanFilter.G);
1215
1216 37109 _kalmanFilter.W(all, all) = noiseScaleMatrix_W(sigma_ra, sigma_rg,
1217 sigma_bad, sigma_bgd,
1218
2/4
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.
 74218 _tau_bad, _tau_bgd);
1219 LOG_DATA("{}: W =\n{}", nameId(), _kalmanFilter.W(all, all));
1220
1221 LOG_DATA("{}: G*W*G^T =\n{}", nameId(), _kalmanFilter.G(all, all) * _kalmanFilter.W(all, all) * _kalmanFilter.G(all, all).transpose());
1222
1223 // 1. Calculate the transition matrix 𝚽_{k-1}
1224 // 2. Calculate the system noise covariance matrix Q_{k-1}
1225
1/2
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
 37109 _kalmanFilter.calcPhiAndQWithVanLoanMethod(tau_i);
1226 }
1227
1228 // If Q was calculated over Van Loan, then the Phi matrix was automatically calculated with the exponential matrix
1229
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)
1230 {
1231 19332 auto calcPhi = [&]() {
1232
2/2
✓ Branch 0 taken 6444 times.
✓ Branch 1 taken 12888 times.
 19332 if (_phiCalculationAlgorithm == PhiCalculationAlgorithm::Exponential)
1233 {
1234 // 1. Calculate the transition matrix 𝚽_{k-1}
1235 6444 _kalmanFilter.calcTransitionMatrix_Phi_exp(tau_i);
1236 }
1237
1/2
✓ Branch 0 taken 12888 times.
✗ Branch 1 not taken.
 12888 else if (_phiCalculationAlgorithm == PhiCalculationAlgorithm::Taylor)
1238 {
1239 // 1. Calculate the transition matrix 𝚽_{k-1}
1240 12888 _kalmanFilter.calcTransitionMatrix_Phi_Taylor(tau_i, static_cast<size_t>(_phiCalculationTaylorOrder));
1241 }
1242 else
1243 {
1244 ✗ LOG_CRITICAL("{}: Calculation algorithm '{}' for the system matrix Phi is not supported.", nameId(), fmt::underlying(_phiCalculationAlgorithm));
1245 }
1246 19332 };
1247
1/2
✓ Branch 1 taken 19332 times.
✗ Branch 2 not taken.
 19332 calcPhi();
1248 }
1249 LOG_DATA("{}: KF.Phi =\n{}", nameId(), _kalmanFilter.Phi);
1250 LOG_DATA("{}: KF.Q =\n{}", nameId(), _kalmanFilter.Q);
1251
1252 LOG_DATA("{}: Q - Q^T =\n{}", nameId(), _kalmanFilter.Q(all, all) - _kalmanFilter.Q(all, all).transpose());
1253 LOG_DATA("{}: KF.P (before prediction) =\n{}", nameId(), _kalmanFilter.P);
1254
1255 // 3. Propagate the state vector estimate from x(+) and x(-)
1256 // 4. Propagate the error covariance matrix from P(+) and P(-)
1257
1/2
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
 49997 _kalmanFilter.predict();
1258
1259 LOG_DATA("{}: KF.x = {}", nameId(), _kalmanFilter.x.transposed());
1260 LOG_DATA("{}: KF.P (after prediction) =\n{}", nameId(), _kalmanFilter.P);
1261
1262 // Averaging of P to avoid numerical problems with symmetry (did not work)
1263 // _kalmanFilter.P = ((_kalmanFilter.P + _kalmanFilter.P.transpose()) / 2.0);
1264
1265 // LOG_DEBUG("{}: F\n{}\n", nameId(), F);
1266 // LOG_DEBUG("{}: Phi\n{}\n", nameId(), _kalmanFilter.Phi);
1267
1268 // LOG_DEBUG("{}: Q\n{}\n", nameId(), _kalmanFilter.Q);
1269 // LOG_DEBUG("{}: Q - Q^T\n{}\n", nameId(), _kalmanFilter.Q - _kalmanFilter.Q.transpose());
1270
1271 // LOG_DEBUG("{}: x\n{}\n", nameId(), _kalmanFilter.x);
1272
1273 // LOG_DEBUG("{}: P\n{}\n", nameId(), _kalmanFilter.P);
1274 // LOG_DEBUG("{}: P - P^T\n{}\n", nameId(), _kalmanFilter.P - _kalmanFilter.P.transpose());
1275
1276
1/2
✓ Branch 0 taken 49997 times.
✗ Branch 1 not taken.
 49997 if (_checkKalmanMatricesRanks)
1277 {
1278
1/2
✓ Branch 2 taken 49997 times.
✗ Branch 3 not taken.
 49997 Eigen::FullPivLU<Eigen::MatrixXd> lu(_kalmanFilter.P(all, all));
1279
1/2
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
 49997 auto rank = lu.rank();
1280
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 49997 times.
 49997 if (rank != _kalmanFilter.P(all, all).rows())
1281 {
1282 ✗ LOG_WARN("{}: [{}] P.rank = {}", nameId(), inertialNavSol->insTime.toYMDHMS(GPST), rank);
1283 }
1284 49997 }
1285 49997 }
1286
1287 26813 void NAV::LooselyCoupledKF::looselyCoupledUpdate(const std::shared_ptr<const PosVel>& posVelObs)
1288 {
1289
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.");
1290
1291 LOG_DATA("{}: [{}] Updating (lastInertial at [{}])", nameId(), posVelObs->insTime.toYMDHMS(GPST), _inertialIntegrator.getLatestState().value().get().insTime.toYMDHMS(GPST));
1292
1293 // -------------------------------------------- GUI Parameters -----------------------------------------------
1294
1295 // Latitude 𝜙, longitude λ and altitude (height above ground) in [rad, rad, m] at the time tₖ₋₁
1296
2/4
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26813 times.
✗ Branch 5 not taken.
 26813 const Eigen::Vector3d& lla_position = _inertialIntegrator.getLatestState().value().get().lla_position();
1297 LOG_DATA("{}: lla_position = {} [rad, rad, m]", nameId(), lla_position.transpose());
1298
1299 // GNSS measurement uncertainty for the position (Variance σ²) in [m^2]
1300
2/4
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26813 times.
✗ Branch 5 not taken.
 26813 Eigen::Vector3d gnssSigmaSquaredPosition = Eigen::Vector3d::Zero();
1301 // GNSS measurement uncertainty for the position (Variance σ²) in [rad^2, rad^2, m^2]
1302
2/4
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26813 times.
✗ Branch 5 not taken.
 26813 Eigen::Vector3d gnssSigmaSquaredLatLonAlt = Eigen::Vector3d::Zero();
1303
4/6
✓ Branch 0 taken 2592 times.
✓ Branch 1 taken 24221 times.
✓ Branch 5 taken 2592 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 26813 times.
✗ Branch 8 not taken.
 26813 if (_gnssMeasurementUncertaintyPositionOverride || !posVelObs->e_positionStdev())
1304 {
1305
1/5
✓ Branch 0 taken 26813 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 26813 switch (_gnssMeasurementUncertaintyPositionUnit)
1306 {
1307 26813 case GnssMeasurementUncertaintyPositionUnit::meter:
1308
3/6
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26813 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26813 times.
✗ Branch 8 not taken.
 26813 gnssSigmaSquaredPosition = _gnssMeasurementUncertaintyPosition.array().pow(2);
1309
6/12
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26813 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26813 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 26813 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 26813 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 26813 times.
✗ Branch 17 not taken.
 26813 gnssSigmaSquaredLatLonAlt = (trafo::ecef2lla_WGS84(trafo::ned2ecef(_gnssMeasurementUncertaintyPosition, lla_position)) - lla_position).array().pow(2);
1310 26813 break;
1311 ✗ case GnssMeasurementUncertaintyPositionUnit::meter2:
1312 ✗ gnssSigmaSquaredPosition = _gnssMeasurementUncertaintyPosition;
1313 ✗ gnssSigmaSquaredLatLonAlt = (trafo::ecef2lla_WGS84(trafo::ned2ecef(_gnssMeasurementUncertaintyPosition.cwiseSqrt(), lla_position)) - lla_position).array().pow(2);
1314 ✗ break;
1315 ✗ case GnssMeasurementUncertaintyPositionUnit::rad_rad_m:
1316 ✗ gnssSigmaSquaredPosition = (trafo::lla2ecef_WGS84(lla_position + _gnssMeasurementUncertaintyPosition) - _inertialIntegrator.getLatestState().value().get().e_position()).array().pow(2);
1317 ✗ gnssSigmaSquaredLatLonAlt = _gnssMeasurementUncertaintyPosition.array().pow(2);
1318 ✗ break;
1319 ✗ case GnssMeasurementUncertaintyPositionUnit::rad2_rad2_m2:
1320 ✗ gnssSigmaSquaredPosition = (trafo::lla2ecef_WGS84(lla_position + _gnssMeasurementUncertaintyPosition.cwiseSqrt()) - _inertialIntegrator.getLatestState().value().get().e_position()).array().pow(2);
1321 ✗ gnssSigmaSquaredLatLonAlt = _gnssMeasurementUncertaintyPosition;
1322 ✗ break;
1323 }
1324 }
1325 else
1326 {
1327 ✗ gnssSigmaSquaredPosition = posVelObs->e_positionStdev()->get().array().pow(2) * 10;
1328 ✗ gnssSigmaSquaredLatLonAlt = (trafo::ecef2lla_WGS84(trafo::ned2ecef(gnssSigmaSquaredPosition, lla_position)) - lla_position).array().pow(2);
1329 }
1330 LOG_DATA("{}: gnssSigmaSquaredPosition = {} [m^2]", nameId(), gnssSigmaSquaredPosition.transpose());
1331 LOG_DATA("{}: gnssSigmaSquaredLatLonAlt = {} [rad^2, rad^2, m^2]", nameId(), gnssSigmaSquaredLatLonAlt.transpose());
1332
1333 // GNSS measurement uncertainty for the velocity (Variance σ²) in [m^2/s^2]
1334
2/4
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26813 times.
✗ Branch 5 not taken.
 26813 Eigen::Vector3d gnssSigmaSquaredVelocity = Eigen::Vector3d::Zero();
1335
4/6
✓ Branch 0 taken 2592 times.
✓ Branch 1 taken 24221 times.
✓ Branch 5 taken 2592 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 26813 times.
✗ Branch 8 not taken.
 26813 if (_gnssMeasurementUncertaintyVelocityOverride || !posVelObs->e_velocityStdev())
1336 {
1337
1/3
✓ Branch 0 taken 26813 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 26813 switch (_gnssMeasurementUncertaintyVelocityUnit)
1338 {
1339 26813 case GnssMeasurementUncertaintyVelocityUnit::m_s:
1340
3/6
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26813 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26813 times.
✗ Branch 8 not taken.
 26813 gnssSigmaSquaredVelocity = _gnssMeasurementUncertaintyVelocity.array().pow(2);
1341 26813 break;
1342 ✗ case GnssMeasurementUncertaintyVelocityUnit::m2_s2:
1343 ✗ gnssSigmaSquaredVelocity = _gnssMeasurementUncertaintyVelocity;
1344 ✗ break;
1345 }
1346 }
1347 else
1348 {
1349 ✗ gnssSigmaSquaredVelocity = posVelObs->e_velocityStdev()->get().array().pow(2) * 10;
1350 }
1351 LOG_DATA("{}: gnssSigmaSquaredVelocity = {} [m^2/S^2]", nameId(), gnssSigmaSquaredVelocity.transpose());
1352
1353 // ---------------------------------------------- Correction -------------------------------------------------
1354
1355
1/2
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
 26813 auto p_omega_ip = _inertialIntegrator.p_calcCurrentAngularRate();
1356 // Angular rate measured in units of [rad/s], and given in the body frame
1357 26813 Eigen::Vector3d b_omega_ip = p_omega_ip
1358
2/4
✓ Branch 0 taken 26813 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 26813 times.
✗ Branch 4 not taken.
 26813 ? _lastImuObs->imuPos.b_quatGyro_p() * p_omega_ip.value()
1359
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();
1360 LOG_DATA("{}: b_omega_ip = {} [rad/s]", nameId(), b_omega_ip.transpose());
1361
1362
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)
1363 {
1364 // Prime vertical radius of curvature (East/West) [m]
1365
2/4
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
 25517 double R_E = calcEarthRadius_E(lla_position(0));
1366 LOG_DATA("{}: R_E = {} [m]", nameId(), R_E);
1367 // Meridian radius of curvature in [m]
1368
2/4
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
 25517 double R_N = calcEarthRadius_N(lla_position(0));
1369 LOG_DATA("{}: R_N = {} [m]", nameId(), R_N);
1370
1371 // Direction Cosine Matrix from body to navigation coordinates, at the time tₖ₋₁
1372
3/6
✓ 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.
 25517 Eigen::Matrix3d n_Dcm_b = _inertialIntegrator.getLatestState().value().get().n_Quat_b().toRotationMatrix();
1373 LOG_DATA("{}: n_Dcm_b =\n{}", nameId(), n_Dcm_b);
1374
1375 // Conversion matrix between cartesian and curvilinear perturbations to the position
1376
1/2
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
 25517 Eigen::Matrix3d T_rn_p = conversionMatrixCartesianCurvilinear(lla_position, R_N, R_E);
1377 LOG_DATA("{}: T_rn_p =\n{}", nameId(), T_rn_p);
1378
1379 // Skew-symmetric matrix of the Earth-rotation vector in local navigation frame axes
1380
5/10
✓ 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.
✓ Branch 14 taken 25517 times.
✗ Branch 15 not taken.
 25517 Eigen::Matrix3d n_Omega_ie = math::skewSymmetricMatrix(_inertialIntegrator.getLatestState().value().get().n_Quat_e() * InsConst::e_omega_ie);
1381 LOG_DATA("{}: n_Omega_ie =\n{}", nameId(), n_Omega_ie);
1382
1383 // 5. Calculate the measurement matrix H_k
1384
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);
1385
1386 // 6. Calculate the measurement noise covariance matrix R_k
1387
1/2
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
 25517 _kalmanFilter.R = n_measurementNoiseCovariance_R(gnssSigmaSquaredLatLonAlt, gnssSigmaSquaredVelocity);
1388
1389 // 8. Formulate the measurement z_k
1390
3/6
✓ Branch 5 taken 25517 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 25517 times.
✗ Branch 9 not taken.
✓ Branch 15 taken 25517 times.
✗ Branch 16 not taken.
 76551 _kalmanFilter.z = n_measurementInnovation_dz(posVelObs->lla_position(), _inertialIntegrator.getLatestState().value().get().lla_position(),
1391
2/4
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
 25517 posVelObs->n_velocity(), _inertialIntegrator.getLatestState().value().get().n_velocity(),
1392
2/4
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
 76551 T_rn_p, _inertialIntegrator.getLatestState().value().get().n_Quat_b(), _b_leverArm_InsGnss, b_omega_ip, n_Omega_ie);
1393 }
1394 else // if (_inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::ECEF)
1395 {
1396 // Direction Cosine Matrix from body to navigation coordinates, at the time tₖ₋₁
1397
3/6
✓ 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.
 1296 Eigen::Matrix3d e_Dcm_b = _inertialIntegrator.getLatestState().value().get().e_Quat_b().toRotationMatrix();
1398 LOG_DATA("{}: e_Dcm_b =\n{}", nameId(), e_Dcm_b);
1399
1400 // Skew-symmetric matrix of the Earth-rotation vector in local navigation frame axes
1401
1/2
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
 1296 Eigen::Matrix3d e_Omega_ie = math::skewSymmetricMatrix(InsConst::e_omega_ie);
1402 LOG_DATA("{}: e_Omega_ie =\n{}", nameId(), e_Omega_ie);
1403
1404 // 5. Calculate the measurement matrix H_k
1405
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);
1406
1407 // 6. Calculate the measurement noise covariance matrix R_k
1408
1/2
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
 1296 _kalmanFilter.R = e_measurementNoiseCovariance_R(gnssSigmaSquaredPosition, gnssSigmaSquaredVelocity);
1409
1410 // 8. Formulate the measurement z_k
1411
3/6
✓ Branch 5 taken 1296 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1296 times.
✗ Branch 9 not taken.
✓ Branch 15 taken 1296 times.
✗ Branch 16 not taken.
 3888 _kalmanFilter.z = e_measurementInnovation_dz(posVelObs->e_position(), _inertialIntegrator.getLatestState().value().get().e_position(),
1412
2/4
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1296 times.
✗ Branch 5 not taken.
 1296 posVelObs->e_velocity(), _inertialIntegrator.getLatestState().value().get().e_velocity(),
1413
2/4
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1296 times.
✗ Branch 5 not taken.
 3888 _inertialIntegrator.getLatestState().value().get().e_Quat_b(), _b_leverArm_InsGnss, b_omega_ip, e_Omega_ie);
1414 }
1415
1416 LOG_DATA("{}: KF.H =\n{}", nameId(), _kalmanFilter.H);
1417 LOG_DATA("{}: KF.R =\n{}", nameId(), _kalmanFilter.R);
1418 LOG_DATA("{}: KF.z =\n{}", nameId(), _kalmanFilter.z);
1419
1420
1/2
✓ Branch 0 taken 26813 times.
✗ Branch 1 not taken.
 26813 if (_checkKalmanMatricesRanks)
1421 {
1422
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));
1423
1/2
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
 26813 auto rank = lu.rank();
1424
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 26813 times.
 26813 if (rank != _kalmanFilter.H(all, all).rows())
1425 {
1426 ✗ LOG_WARN("{}: [{}] (HPH^T + R).rank = {}", nameId(), posVelObs->insTime.toYMDHMS(GPST), rank);
1427 }
1428 26813 }
1429
1430 // 7. Calculate the Kalman gain matrix K_k
1431 // 9. Update the state vector estimate from x(-) to x(+)
1432 // 10. Update the error covariance matrix from P(-) to P(+
1433
1/2
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
 26813 _kalmanFilter.correctWithMeasurementInnovation();
1434
1435 LOG_DATA("{}: KF.K =\n{}", nameId(), _kalmanFilter.K);
1436 LOG_DATA("{}: KF.x =\n{}", nameId(), _kalmanFilter.x);
1437 LOG_DATA("{}: KF.P =\n{}", nameId(), _kalmanFilter.P);
1438
1439 // Averaging of P to avoid numerical problems with symmetry (did not work)
1440 // _kalmanFilter.P = ((_kalmanFilter.P + _kalmanFilter.P.transpose()) / 2.0);
1441
1442
1/2
✓ Branch 0 taken 26813 times.
✗ Branch 1 not taken.
 26813 if (_checkKalmanMatricesRanks)
1443 {
1444
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));
1445
1/2
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
 26813 auto rank = lu.rank();
1446
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 26813 times.
 26813 if (rank != _kalmanFilter.H(all, all).rows())
1447 {
1448 ✗ LOG_WARN("{}: [{}] (HPH^T + R).rank = {}", nameId(), posVelObs->insTime.toYMDHMS(GPST), rank);
1449 }
1450
1451
1/2
✓ Branch 2 taken 26813 times.
✗ Branch 3 not taken.
 26813 Eigen::FullPivLU<Eigen::MatrixXd> luP(_kalmanFilter.P(all, all));
1452
1/2
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
 26813 rank = luP.rank();
1453
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 26813 times.
 26813 if (rank != _kalmanFilter.P(all, all).rows())
1454 {
1455 ✗ LOG_WARN("{}: [{}] P.rank = {}", nameId(), posVelObs->insTime.toYMDHMS(GPST), rank);
1456 }
1457 26813 }
1458
1459 // LOG_DEBUG("{}: H\n{}\n", nameId(), _kalmanFilter.H);
1460 // LOG_DEBUG("{}: R\n{}\n", nameId(), _kalmanFilter.R);
1461 // LOG_DEBUG("{}: z =\n{}", nameId(), _kalmanFilter.z.transposed());
1462
1463 // LOG_DEBUG("{}: K\n{}\n", nameId(), _kalmanFilter.K);
1464 // LOG_DEBUG("{}: x =\n{}", nameId(), _kalmanFilter.x.transposed());
1465 // LOG_DEBUG("{}: P\n{}\n", nameId(), _kalmanFilter.P);
1466
1467 // LOG_DEBUG("{}: K * z =\n{}", nameId(), (_kalmanFilter.K(all, all) * _kalmanFilter.z(all)).transpose());
1468
1469 // LOG_DEBUG("{}: P - P^T\n{}\n", nameId(), _kalmanFilter.P(all, all) - _kalmanFilter.P(all, all).transpose());
1470
1471 // Push out the new data
1472
1/2
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
 26813 auto lckfSolution = std::make_shared<InsGnssLCKFSolution>();
1473 26813 lckfSolution->insTime = posVelObs->insTime;
1474
2/4
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 26813 times.
✗ Branch 6 not taken.
 26813 lckfSolution->positionError = _kalmanFilter.x.segment<3>(KFPos);
1475
2/4
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 26813 times.
✗ Branch 6 not taken.
 26813 lckfSolution->velocityError = _kalmanFilter.x.segment<3>(KFVel);
1476
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->attitudeError = _kalmanFilter.x.segment<3>(KFAtt) * (1. / SCALE_FACTOR_ATTITUDE);
1477
1478 LOG_DATA("{}: Accumulated biases before error has been applied: b_biasAccel = {}, b_biasGyro = {}", nameId(), _inertialIntegrator.p_getLastAccelerationBias().transpose(), _inertialIntegrator.p_getLastAngularRateBias().transpose());
1479
1480
8/16
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26813 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26813 times.
✗ Branch 8 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.
✓ Branch 20 taken 26813 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 26813 times.
✗ Branch 24 not taken.
 26813 _inertialIntegrator.applySensorBiasesIncrements(_lastImuObs->imuPos.p_quatAccel_b() * -_kalmanFilter.x.segment<3>(KFAccBias) * (1. / SCALE_FACTOR_ACCELERATION),
1481
5/10
✓ 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.
✓ Branch 11 taken 26813 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 26813 times.
✗ Branch 15 not taken.
 26813 _lastImuObs->imuPos.p_quatGyro_b() * -_kalmanFilter.x.segment<3>(KFGyrBias) * (1. / SCALE_FACTOR_ANGULAR_RATE));
1482
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 = -_inertialIntegrator.p_getLastAccelerationBias();
1483
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 = -_inertialIntegrator.p_getLastAngularRateBias();
1484
1485 LOG_DATA("{}: Biases after error has been applied: b_biasAccel = {}, b_biasGyro = {}", nameId(), lckfSolution->b_biasAccel.transpose(), lckfSolution->b_biasGyro.transpose());
1486
1487
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)
1488 {
1489
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 lckfSolution->positionError = lckfSolution->positionError.array() * Eigen::Array3d(1. / SCALE_FACTOR_LAT_LON, 1. / SCALE_FACTOR_LAT_LON, 1);
1490 25517 lckfSolution->frame = InsGnssLCKFSolution::Frame::NED;
1491
1/2
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
 25517 _inertialIntegrator.applyStateErrors_n(lckfSolution->positionError, lckfSolution->velocityError, lckfSolution->attitudeError);
1492
4/8
✓ Branch 2 taken 25517 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 25517 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 25517 times.
✗ Branch 9 not taken.
✓ Branch 13 taken 25517 times.
✗ Branch 14 not taken.
 51034 lckfSolution->setStateAndStdDev_n(_inertialIntegrator.getLatestState().value().get().lla_position(), _kalmanFilter.P(KFPos, KFPos),
1493
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.
 51034 _inertialIntegrator.getLatestState().value().get().n_velocity(), _kalmanFilter.P(KFVel, KFVel),
1494
2/4
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
 25517 _inertialIntegrator.getLatestState().value().get().n_Quat_b());
1495
2/4
✓ Branch 2 taken 25517 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 25517 times.
✗ Branch 6 not taken.
 25517 lckfSolution->setPosVelCovarianceMatrix_n(_kalmanFilter.P(KFPosVel, KFPosVel));
1496 }
1497 else // if (_inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::ECEF)
1498 {
1499 1296 lckfSolution->frame = InsGnssLCKFSolution::Frame::ECEF;
1500
1/2
✓ Branch 4 taken 1296 times.
✗ Branch 5 not taken.
 1296 _inertialIntegrator.applyStateErrors_e(lckfSolution->positionError, lckfSolution->velocityError, lckfSolution->attitudeError);
1501
4/8
✓ Branch 2 taken 1296 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1296 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1296 times.
✗ Branch 9 not taken.
✓ Branch 13 taken 1296 times.
✗ Branch 14 not taken.
 2592 lckfSolution->setStateAndStdDev_e(_inertialIntegrator.getLatestState().value().get().e_position(), _kalmanFilter.P(KFPos, KFPos),
1502
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.
 2592 _inertialIntegrator.getLatestState().value().get().e_velocity(), _kalmanFilter.P(KFVel, KFVel),
1503
2/4
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1296 times.
✗ Branch 5 not taken.
 1296 _inertialIntegrator.getLatestState().value().get().e_Quat_b());
1504
2/4
✓ Branch 2 taken 1296 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1296 times.
✗ Branch 6 not taken.
 1296 lckfSolution->setPosVelCovarianceMatrix_e(_kalmanFilter.P(KFPosVel, KFPosVel));
1505 }
1506
1507 // Closed loop
1508
1/2
✓ Branch 2 taken 26813 times.
✗ Branch 3 not taken.
 26813 _kalmanFilter.x(all).setZero();
1509
1510 LOG_DATA("{}: [{}] Sending out updated solution", nameId(), lckfSolution->insTime.toYMDHMS(GPST));
1511
1/2
✓ Branch 2 taken 26813 times.
✗ Branch 3 not taken.
 26813 invokeCallbacks(OUTPUT_PORT_INDEX_SOLUTION, lckfSolution);
1512 26813 }
1513
1514 // ###########################################################################################################
1515 // System matrix 𝐅
1516 // ###########################################################################################################
1517
1518 NAV::KeyedMatrix<double, NAV::LooselyCoupledKF::KFStates, NAV::LooselyCoupledKF::KFStates, 15, 15>
1519 37109 NAV::LooselyCoupledKF::n_systemMatrix_F(const Eigen::Quaterniond& n_Quat_b,
1520 const Eigen::Vector3d& b_specForce_ib,
1521 const Eigen::Vector3d& n_omega_in,
1522 const Eigen::Vector3d& n_velocity,
1523 const Eigen::Vector3d& lla_position,
1524 double R_N,
1525 double R_E,
1526 double g_0,
1527 double r_eS_e,
1528 const Eigen::Vector3d& tau_bad,
1529 const Eigen::Vector3d& tau_bgd) const
1530 {
1531
1/2
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
 37109 double latitude = lla_position(0); // Geodetic latitude of the body in [rad]
1532
1/2
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
 37109 double altitude = lla_position(2); // Geodetic height of the body in [m]
1533
1534
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)
1535
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)
1536
1537 // System matrix 𝐅
1538 // 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}
1539
2/4
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.
 37109 KeyedMatrix<double, KFStates, KFStates, 15, 15> F(Eigen::Matrix<double, 15, 15>::Zero(), States);
1540
1541
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 F.block<3>(KFAtt, KFAtt) = n_F_dpsi_dpsi(n_omega_in);
1542
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 F.block<3>(KFAtt, KFVel) = n_F_dpsi_dv(latitude, altitude, R_N, R_E);
1543
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 F.block<3>(KFAtt, KFPos) = n_F_dpsi_dr(latitude, altitude, n_velocity, R_N, R_E);
1544
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 F.block<3>(KFAtt, KFGyrBias) = n_F_dpsi_dw(n_Quat_b.toRotationMatrix());
1545
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 F.block<3>(KFVel, KFAtt) = n_F_dv_dpsi(n_Quat_b * b_specForce_ib);
1546
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 F.block<3>(KFVel, KFVel) = n_F_dv_dv(n_velocity, latitude, altitude, R_N, R_E);
1547
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 F.block<3>(KFVel, KFPos) = n_F_dv_dr(n_velocity, latitude, altitude, R_N, R_E, g_0, r_eS_e);
1548
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 F.block<3>(KFVel, KFAccBias) = n_F_dv_df(n_Quat_b.toRotationMatrix());
1549
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 F.block<3>(KFPos, KFVel) = n_F_dr_dv(latitude, altitude, R_N, R_E);
1550
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 F.block<3>(KFPos, KFPos) = n_F_dr_dr(n_velocity, latitude, altitude, R_N, R_E);
1551
2/2
✓ Branch 0 taken 30665 times.
✓ Branch 1 taken 6444 times.
 37109 if (_qCalculationAlgorithm == QCalculationAlgorithm::VanLoan)
1552 {
1553
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 15 taken 30665 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 30665 times.
✗ Branch 19 not taken.
 30665 F.block<3>(KFAccBias, KFAccBias) = n_F_df_df(_randomProcessAccel == RandomProcess::RandomWalk ? Eigen::Vector3d::Zero() : beta_bad);
1554
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 15 taken 30665 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 30665 times.
✗ Branch 19 not taken.
 30665 F.block<3>(KFGyrBias, KFGyrBias) = n_F_dw_dw(_randomProcessGyro == RandomProcess::RandomWalk ? Eigen::Vector3d::Zero() : beta_bgd);
1555 }
1556
1557
2/4
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.
 37109 F.middleRows<3>(KFAtt) *= SCALE_FACTOR_ATTITUDE; // 𝜓' [deg / s] = 180/π * ... [rad / s]
1558
2/4
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.
 37109 F.middleCols<3>(KFAtt) *= 1. / SCALE_FACTOR_ATTITUDE;
1559
1560 // F.middleRows<3>(Vel) *= 1.; // 𝛿v' [m / s^2] = 1 * [m / s^2]
1561 // F.middleCols<3>(Vel) *= 1. / 1.;
1562
1563
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 F.middleRows<2>({ PosLat, PosLon }) *= SCALE_FACTOR_LAT_LON; // 𝛿ϕ' [pseudometre / s] = R0 * [rad / s]
1564
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 F.middleCols<2>({ PosLat, PosLon }) *= 1. / SCALE_FACTOR_LAT_LON;
1565 // F.row(PosAlt) *= 1.; // 𝛿h' [m / s] = 1 * [m / s]
1566 // F.col(PosAlt) *= 1. / 1.;
1567
1568
2/4
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.
 37109 F.middleRows<3>(KFAccBias) *= SCALE_FACTOR_ACCELERATION; // 𝛿f' [mg / s] = 1e3 / g * [m / s^3]
1569
2/4
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.
 37109 F.middleCols<3>(KFAccBias) *= 1. / SCALE_FACTOR_ACCELERATION;
1570
1571
2/4
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.
 37109 F.middleRows<3>(KFGyrBias) *= SCALE_FACTOR_ANGULAR_RATE; // 𝛿ω' [mrad / s^2] = 1e3 * [rad / s^2]
1572
2/4
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.
 37109 F.middleCols<3>(KFGyrBias) *= 1. / SCALE_FACTOR_ANGULAR_RATE;
1573
1574 74218 return F;
1575 ✗ }
1576
1577 NAV::KeyedMatrix<double, NAV::LooselyCoupledKF::KFStates, NAV::LooselyCoupledKF::KFStates, 15, 15>
1578 12888 NAV::LooselyCoupledKF::e_systemMatrix_F(const Eigen::Quaterniond& e_Quat_b,
1579 const Eigen::Vector3d& b_specForce_ib,
1580 const Eigen::Vector3d& e_position,
1581 const Eigen::Vector3d& e_gravitation,
1582 double r_eS_e,
1583 const Eigen::Vector3d& e_omega_ie,
1584 const Eigen::Vector3d& tau_bad,
1585 const Eigen::Vector3d& tau_bgd) const
1586 {
1587
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)
1588
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)
1589
1590 // System matrix 𝐅
1591 // 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}
1592
2/4
✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12888 times.
✗ Branch 5 not taken.
 12888 KeyedMatrix<double, KFStates, KFStates, 15, 15> F(Eigen::Matrix<double, 15, 15>::Zero(), States);
1593
1594
4/8
✓ 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.
 12888 F.block<3>(KFAtt, KFAtt) = e_F_dpsi_dpsi(e_omega_ie.z());
1595
4/8
✓ 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.
 12888 F.block<3>(KFAtt, KFGyrBias) = e_F_dpsi_dw(e_Quat_b.toRotationMatrix());
1596
4/8
✓ 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.
 12888 F.block<3>(KFVel, KFAtt) = e_F_dv_dpsi(e_Quat_b * b_specForce_ib);
1597
4/8
✓ 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.
 12888 F.block<3>(KFVel, KFVel) = e_F_dv_dv(e_omega_ie.z());
1598
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 F.block<3>(KFVel, KFPos) = e_F_dv_dr(e_position, e_gravitation, r_eS_e, e_omega_ie);
1599
4/8
✓ 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.
 12888 F.block<3>(KFVel, KFAccBias) = e_F_dv_df(e_Quat_b.toRotationMatrix());
1600
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 F.block<3>(KFPos, KFVel) = e_F_dr_dv();
1601
2/2
✓ Branch 0 taken 6444 times.
✓ Branch 1 taken 6444 times.
 12888 if (_qCalculationAlgorithm == QCalculationAlgorithm::VanLoan)
1602 {
1603
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 15 taken 6444 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6444 times.
✗ Branch 19 not taken.
 6444 F.block<3>(KFAccBias, KFAccBias) = e_F_df_df(_randomProcessAccel == RandomProcess::RandomWalk ? Eigen::Vector3d::Zero() : beta_bad);
1604
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 15 taken 6444 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6444 times.
✗ Branch 19 not taken.
 6444 F.block<3>(KFGyrBias, KFGyrBias) = e_F_dw_dw(_randomProcessGyro == RandomProcess::RandomWalk ? Eigen::Vector3d::Zero() : beta_bgd);
1605 }
1606
1607
2/4
✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12888 times.
✗ Branch 5 not taken.
 12888 F.middleRows<3>(KFAtt) *= SCALE_FACTOR_ATTITUDE; // 𝜓' [deg / s] = 180/π * ... [rad / s]
1608
2/4
✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12888 times.
✗ Branch 5 not taken.
 12888 F.middleCols<3>(KFAtt) *= 1. / SCALE_FACTOR_ATTITUDE;
1609
1610 // F.middleRows<3>(Vel) *= 1.; // 𝛿v' [m / s^2] = 1 * [m / s^2]
1611 // F.middleCols<3>(Vel) *= 1. / 1.;
1612
1613 // F.middleRows<3>(Pos) *= 1.; // 𝛿r' [m / s] = 1 * [m / s]
1614 // F.middleCols<3>(Pos) *= 1. / 1.;
1615
1616
2/4
✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12888 times.
✗ Branch 5 not taken.
 12888 F.middleRows<3>(KFAccBias) *= SCALE_FACTOR_ACCELERATION; // 𝛿f' [mg / s] = 1e3 / g * [m / s^3]
1617
2/4
✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12888 times.
✗ Branch 5 not taken.
 12888 F.middleCols<3>(KFAccBias) *= 1. / SCALE_FACTOR_ACCELERATION;
1618
1619
2/4
✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12888 times.
✗ Branch 5 not taken.
 12888 F.middleRows<3>(KFGyrBias) *= SCALE_FACTOR_ANGULAR_RATE; // 𝛿ω' [mrad / s^2] = 1e3 * [rad / s^2]
1620
2/4
✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12888 times.
✗ Branch 5 not taken.
 12888 F.middleCols<3>(KFGyrBias) *= 1. / SCALE_FACTOR_ANGULAR_RATE;
1621
1622 25776 return F;
1623 ✗ }
1624
1625 // ###########################################################################################################
1626 // Noise input matrix 𝐆 & Noise scale matrix 𝐖
1627 // System noise covariance matrix 𝐐
1628 // ###########################################################################################################
1629
1630 NAV::KeyedMatrix<double, NAV::LooselyCoupledKF::KFStates, NAV::LooselyCoupledKF::KFStates, 15, 15>
1631 37109 NAV::LooselyCoupledKF::noiseInputMatrix_G(const Eigen::Quaterniond& ien_Quat_b)
1632 {
1633 // DCM matrix from body to navigation frame
1634
1/2
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
 37109 Eigen::Matrix3d ien_Dcm_b = ien_Quat_b.toRotationMatrix();
1635
1636 // 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}
1637
2/4
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.
 37109 KeyedMatrix<double, KFStates, KFStates, 15, 15> G(Eigen::Matrix<double, 15, 15>::Zero(), States, States);
1638
1639
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 G.block<3>(KFAtt, KFAtt) = SCALE_FACTOR_ATTITUDE * ien_Dcm_b;
1640
2/4
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.
 37109 G.block<3>(KFVel, KFVel) = ien_Dcm_b;
1641
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 G.block<3>(KFAccBias, KFAccBias) = SCALE_FACTOR_ACCELERATION * Eigen::Matrix3d::Identity();
1642
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 G.block<3>(KFGyrBias, KFGyrBias) = SCALE_FACTOR_ANGULAR_RATE * Eigen::Matrix3d::Identity();
1643
1644 74218 return G;
1645 ✗ }
1646
1647 37109 Eigen::Matrix<double, 15, 15> NAV::LooselyCoupledKF::noiseScaleMatrix_W(const Eigen::Vector3d& sigma_ra, const Eigen::Vector3d& sigma_rg,
1648 const Eigen::Vector3d& sigma_bad, const Eigen::Vector3d& sigma_bgd,
1649 const Eigen::Vector3d& tau_bad, const Eigen::Vector3d& tau_bgd)
1650 {
1651
2/4
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.
 37109 Eigen::Matrix<double, 15, 15> W = Eigen::Matrix<double, 15, 15>::Zero();
1652
1653
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(),
1654
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(),
1655
2/4
✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.
 37109 Eigen::Vector3d::Zero(),
1656
10/18
✓ Branch 0 taken 24221 times.
✓ Branch 1 taken 12888 times.
✓ Branch 3 taken 24221 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 12888 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 12888 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 37109 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 37109 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 37109 times.
✗ Branch 25 not taken.
 37109 (_randomProcessAccel == RandomProcess::RandomWalk ? sigma_bad : psdBiasGaussMarkov(sigma_bad.array().square(), tau_bad)).array().square(), // S_bad
1657
10/18
✓ Branch 0 taken 24221 times.
✓ Branch 1 taken 12888 times.
✓ Branch 3 taken 24221 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 12888 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 12888 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 37109 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 37109 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 37109 times.
✗ Branch 25 not taken.
 37109 (_randomProcessGyro == RandomProcess::RandomWalk ? sigma_bgd : psdBiasGaussMarkov(sigma_bgd.array().square(), tau_bgd)).array().square(); // S_bgd
1658
1659 37109 return W;
1660 }
1661
1662 NAV::KeyedMatrix<double, NAV::LooselyCoupledKF::KFStates, NAV::LooselyCoupledKF::KFStates, 15, 15>
1663 6444 NAV::LooselyCoupledKF::n_systemNoiseCovarianceMatrix_Q(const Eigen::Vector3d& sigma2_ra, const Eigen::Vector3d& sigma2_rg,
1664 const Eigen::Vector3d& sigma2_bad, const Eigen::Vector3d& sigma2_bgd,
1665 const Eigen::Vector3d& tau_bad, const Eigen::Vector3d& tau_bgd,
1666 const Eigen::Matrix3d& n_F_21, const Eigen::Matrix3d& T_rn_p,
1667 const Eigen::Matrix3d& n_Dcm_b, const double& tau_s)
1668 {
1669 // 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)
1670
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;
1671
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;
1672
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();
1673
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();
1674
1675
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();
1676
1677
2/4
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6444 times.
✗ Branch 5 not taken.
 6444 KeyedMatrix<double, KFStates, KFStates, 15, 15> Q(Eigen::Matrix<double, 15, 15>::Zero(), States, States);
1678
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.
 6444 Q.block<3>(KFAtt, KFAtt) = Q_psi_psi(S_rg, S_bgd, tau_s); // Q_11
1679
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.
 6444 Q.block<3>(KFVel, KFAtt) = ien_Q_dv_psi(S_rg, S_bgd, n_F_21, tau_s); // Q_21
1680
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.
 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
1681
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.
 6444 Q.block<3>(KFVel, KFGyrBias) = ien_Q_dv_domega(S_bgd, n_F_21, n_Dcm_b, tau_s); // Q_25
1682
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.
 6444 Q.block<3>(KFPos, KFAtt) = n_Q_dr_psi(S_rg, S_bgd, n_F_21, T_rn_p, tau_s); // Q_31
1683
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.
 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
1684
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.
 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
1685
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.
 6444 Q.block<3>(KFPos, KFAccBias) = n_Q_dr_df(S_bgd, T_rn_p, n_Dcm_b, tau_s); // Q_34
1686
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.
 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
1687
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.
 6444 Q.block<3>(KFAccBias, KFVel) = Q_df_dv(S_bad, b_Dcm_n, tau_s); // Q_42
1688
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.
 6444 Q.block<3>(KFAccBias, KFAccBias) = Q_df_df(S_bad, tau_s); // Q_44
1689
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.
 6444 Q.block<3>(KFGyrBias, KFAtt) = Q_domega_psi(S_bgd, b_Dcm_n, tau_s); // Q_51
1690
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.
 6444 Q.block<3>(KFGyrBias, KFGyrBias) = Q_domega_domega(S_bgd, tau_s); // Q_55
1691
1692
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 Q.block<3>(KFAtt, KFVel) = Q.block<3>(KFVel, KFAtt).transpose(); // Q_21^T
1693
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 Q.block<3>(KFAtt, KFPos) = Q.block<3>(KFPos, KFAtt).transpose(); // Q_31^T
1694
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 Q.block<3>(KFVel, KFPos) = Q.block<3>(KFPos, KFVel).transpose(); // Q_32^T
1695
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 Q.block<3>(KFAccBias, KFPos) = Q.block<3>(KFPos, KFAccBias).transpose(); // Q_34^T
1696
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 Q.block<3>(KFGyrBias, KFVel) = Q.block<3>(KFVel, KFGyrBias).transpose(); // Q_25^T
1697
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 Q.block<3>(KFGyrBias, KFPos) = Q.block<3>(KFPos, KFGyrBias).transpose(); // Q_35^T
1698
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 Q.block<3>(KFVel, KFAccBias) = Q.block<3>(KFAccBias, KFVel).transpose(); // Q_42^T
1699
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 Q.block<3>(KFAtt, KFGyrBias) = Q.block<3>(KFGyrBias, KFAtt).transpose(); // Q_51^T
1700
1701
2/4
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6444 times.
✗ Branch 5 not taken.
 6444 Q.middleRows<3>(KFAtt) *= SCALE_FACTOR_ATTITUDE;
1702
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 Q.middleRows<2>({ PosLat, PosLon }) *= SCALE_FACTOR_LAT_LON;
1703
2/4
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6444 times.
✗ Branch 5 not taken.
 6444 Q.middleRows<3>(KFAccBias) *= SCALE_FACTOR_ACCELERATION;
1704
2/4
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6444 times.
✗ Branch 5 not taken.
 6444 Q.middleRows<3>(KFGyrBias) *= SCALE_FACTOR_ANGULAR_RATE;
1705
1706
2/4
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6444 times.
✗ Branch 5 not taken.
 6444 Q.middleCols<3>(KFAtt) *= SCALE_FACTOR_ATTITUDE;
1707
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 Q.middleCols<2>({ PosLat, PosLon }) *= SCALE_FACTOR_LAT_LON;
1708
2/4
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6444 times.
✗ Branch 5 not taken.
 6444 Q.middleCols<3>(KFAccBias) *= SCALE_FACTOR_ACCELERATION;
1709
2/4
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6444 times.
✗ Branch 5 not taken.
 6444 Q.middleCols<3>(KFGyrBias) *= SCALE_FACTOR_ANGULAR_RATE;
1710
1711 12888 return Q;
1712 ✗ }
1713
1714 NAV::KeyedMatrix<double, NAV::LooselyCoupledKF::KFStates, NAV::LooselyCoupledKF::KFStates, 15, 15>
1715 6444 NAV::LooselyCoupledKF::e_systemNoiseCovarianceMatrix_Q(const Eigen::Vector3d& sigma2_ra, const Eigen::Vector3d& sigma2_rg,
1716 const Eigen::Vector3d& sigma2_bad, const Eigen::Vector3d& sigma2_bgd,
1717 const Eigen::Vector3d& tau_bad, const Eigen::Vector3d& tau_bgd,
1718 const Eigen::Matrix3d& e_F_21,
1719 const Eigen::Matrix3d& e_Dcm_b, const double& tau_s)
1720 {
1721 // 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)
1722
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;
1723
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;
1724
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();
1725
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();
1726
1727
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();
1728
1729
2/4
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6444 times.
✗ Branch 5 not taken.
 6444 KeyedMatrix<double, KFStates, KFStates, 15, 15> Q(Eigen::Matrix<double, 15, 15>::Zero(), States, States);
1730
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.
 6444 Q.block<3>(KFAtt, KFAtt) = Q_psi_psi(S_rg, S_bgd, tau_s); // Q_11
1731
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.
 6444 Q.block<3>(KFVel, KFAtt) = ien_Q_dv_psi(S_rg, S_bgd, e_F_21, tau_s); // Q_21
1732
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.
 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
1733
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.
 6444 Q.block<3>(KFVel, KFGyrBias) = ien_Q_dv_domega(S_bgd, e_F_21, e_Dcm_b, tau_s); // Q_25
1734
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.
 6444 Q.block<3>(KFPos, KFAtt) = ie_Q_dr_psi(S_rg, S_bgd, e_F_21, tau_s); // Q_31
1735
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.
 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
1736
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.
 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
1737
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.
 6444 Q.block<3>(KFPos, KFAccBias) = ie_Q_dr_df(S_bgd, e_Dcm_b, tau_s); // Q_34
1738
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.
 6444 Q.block<3>(KFPos, KFGyrBias) = ie_Q_dr_domega(S_bgd, e_F_21, e_Dcm_b, tau_s); // Q_35
1739
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.
 6444 Q.block<3>(KFAccBias, KFVel) = Q_df_dv(S_bad, b_Dcm_e, tau_s); // Q_42
1740
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.
 6444 Q.block<3>(KFAccBias, KFAccBias) = Q_df_df(S_bad, tau_s); // Q_44
1741
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.
 6444 Q.block<3>(KFGyrBias, KFAtt) = Q_domega_psi(S_bgd, b_Dcm_e, tau_s); // Q_51
1742
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.
 6444 Q.block<3>(KFGyrBias, KFGyrBias) = Q_domega_domega(S_bgd, tau_s); // Q_55
1743
1744
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 Q.block<3>(KFAtt, KFVel) = Q.block<3>(KFVel, KFAtt).transpose(); // Q_21^T
1745
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 Q.block<3>(KFAtt, KFPos) = Q.block<3>(KFPos, KFAtt).transpose(); // Q_31^T
1746
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 Q.block<3>(KFVel, KFPos) = Q.block<3>(KFPos, KFVel).transpose(); // Q_32^T
1747
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 Q.block<3>(KFAccBias, KFPos) = Q.block<3>(KFPos, KFAccBias).transpose(); // Q_34^T
1748
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 Q.block<3>(KFGyrBias, KFVel) = Q.block<3>(KFVel, KFGyrBias).transpose(); // Q_25^T
1749
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 Q.block<3>(KFGyrBias, KFPos) = Q.block<3>(KFPos, KFGyrBias).transpose(); // Q_35^T
1750
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 Q.block<3>(KFVel, KFAccBias) = Q.block<3>(KFAccBias, KFVel).transpose(); // Q_42^T
1751
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 Q.block<3>(KFAtt, KFGyrBias) = Q.block<3>(KFGyrBias, KFAtt).transpose(); // Q_51^T
1752
1753
2/4
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6444 times.
✗ Branch 5 not taken.
 6444 Q.middleRows<3>(KFAtt) *= SCALE_FACTOR_ATTITUDE;
1754
2/4
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6444 times.
✗ Branch 5 not taken.
 6444 Q.middleRows<3>(KFAccBias) *= SCALE_FACTOR_ACCELERATION;
1755
2/4
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6444 times.
✗ Branch 5 not taken.
 6444 Q.middleRows<3>(KFGyrBias) *= SCALE_FACTOR_ANGULAR_RATE;
1756
1757
2/4
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6444 times.
✗ Branch 5 not taken.
 6444 Q.middleCols<3>(KFAtt) *= SCALE_FACTOR_ATTITUDE;
1758
2/4
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6444 times.
✗ Branch 5 not taken.
 6444 Q.middleCols<3>(KFAccBias) *= SCALE_FACTOR_ACCELERATION;
1759
2/4
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6444 times.
✗ Branch 5 not taken.
 6444 Q.middleCols<3>(KFGyrBias) *= SCALE_FACTOR_ANGULAR_RATE;
1760
1761 12888 return Q;
1762 ✗ }
1763
1764 // ###########################################################################################################
1765 // Error covariance matrix P
1766 // ###########################################################################################################
1767
1768 NAV::KeyedMatrix<double, NAV::LooselyCoupledKF::KFStates, NAV::LooselyCoupledKF::KFStates, 15, 15>
1769 51 NAV::LooselyCoupledKF::initialErrorCovarianceMatrix_P0(const Eigen::Vector3d& variance_angles,
1770 const Eigen::Vector3d& variance_vel,
1771 const Eigen::Vector3d& variance_pos,
1772 const Eigen::Vector3d& variance_accelBias,
1773 const Eigen::Vector3d& variance_gyroBias) const
1774 {
1775
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;
1776
1777 // 𝐏 Error covariance matrix
1778
2/4
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
 51 Eigen::Matrix<double, 15, 15> P = Eigen::Matrix<double, 15, 15>::Zero();
1779
1780
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
1781
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 variance_vel, // Velocity covariance
1782
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
1783
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
1784
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
1785
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
1786
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_ANGULAR_RATE, 2) * variance_gyroBias; // Gyroscope Bias covariance
1787
1788
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 102 return { P, States };
1789 }
1790
1791 // ###########################################################################################################
1792 // Correction
1793 // ###########################################################################################################
1794
1795 NAV::KeyedMatrix<double, NAV::LooselyCoupledKF::KFMeas, NAV::LooselyCoupledKF::KFStates, 6, 15>
1796 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)
1797 {
1798 // 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)
1799 // G denotes GNSS indicated
1800
1801
2/4
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
 25517 NAV::KeyedMatrix<double, NAV::LooselyCoupledKF::KFMeas, NAV::LooselyCoupledKF::KFStates, 6, 15> H(Eigen::Matrix<double, 6, 15>::Zero(), Meas, States);
1802
1803 // 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)
1804
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 H.block<3>(dPos, KFAtt) = T_rn_p * math::skewSymmetricMatrix(n_Dcm_b * b_leverArm_InsGnss);
1805
4/8
✓ 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.
 25517 H.block<3>(dPos, KFPos) = -Eigen::Matrix3d::Identity();
1806 // 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)
1807
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 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);
1808
4/8
✓ 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.
 25517 H.block<3>(dVel, KFVel) = -Eigen::Matrix3d::Identity();
1809 // Math: \mathbf{H}_{v5}^n = \mathbf{C}_b^n \begin{bmatrix} \mathbf{l}_{ba}^b \wedge \end{bmatrix} \qquad \text{P. Groves}\,(14.114)
1810
4/8
✓ 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.
 25517 H.block<3>(dVel, KFGyrBias) = n_Dcm_b * math::skewSymmetricMatrix(b_leverArm_InsGnss);
1811
1812
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.
 51034 H.middleRows<2>({ dPosLat, dPosLon }) *= SCALE_FACTOR_LAT_LON;
1813
1814
2/4
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
 25517 H.middleCols<3>(KFAtt) *= 1. / SCALE_FACTOR_ATTITUDE;
1815
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.
 51034 H.middleCols<2>({ PosLat, PosLon }) *= 1. / SCALE_FACTOR_LAT_LON;
1816 // H.middleCols<3>(AccBias) *= 1. / SCALE_FACTOR_ACCELERATION; // Only zero elements
1817
2/4
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
 25517 H.middleCols<3>(KFGyrBias) *= 1. / SCALE_FACTOR_ANGULAR_RATE;
1818
1819 25517 return H;
1820 ✗ }
1821
1822 NAV::KeyedMatrix<double, NAV::LooselyCoupledKF::KFMeas, NAV::LooselyCoupledKF::KFStates, 6, 15>
1823 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)
1824 {
1825 // 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)
1826 // G denotes GNSS indicated
1827
1828
2/4
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1296 times.
✗ Branch 5 not taken.
 1296 NAV::KeyedMatrix<double, NAV::LooselyCoupledKF::KFMeas, NAV::LooselyCoupledKF::KFStates, 6, 15> H(Eigen::Matrix<double, 6, 15>::Zero(), Meas, States);
1829
1830 // 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)
1831
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 H.block<3>(dPos, KFAtt) = math::skewSymmetricMatrix(e_Dcm_b * b_leverArm_InsGnss);
1832
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 H.block<3>(dPos, KFPos) = -Eigen::Matrix3d::Identity();
1833 // 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)
1834
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 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);
1835
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 H.block<3>(dVel, KFVel) = -Eigen::Matrix3d::Identity();
1836 // Math: \mathbf{H}_{v5}^e = \mathbf{C}_b^e \begin{bmatrix} \mathbf{l}_{ba}^b \wedge \end{bmatrix} \qquad \text{P. Groves}\,(14.114)
1837
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 H.block<3>(dVel, KFGyrBias) = e_Dcm_b * math::skewSymmetricMatrix(b_leverArm_InsGnss);
1838
1839
2/4
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1296 times.
✗ Branch 5 not taken.
 1296 H.middleCols<3>(KFAtt) *= 1. / SCALE_FACTOR_ATTITUDE;
1840 // H.middleCols<3>(AccBias) *= 1. / SCALE_FACTOR_ACCELERATION; // Only zero elements
1841
2/4
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1296 times.
✗ Branch 5 not taken.
 1296 H.middleCols<3>(KFGyrBias) *= 1. / SCALE_FACTOR_ANGULAR_RATE;
1842
1843 1296 return H;
1844 ✗ }
1845
1846 NAV::KeyedMatrix<double, NAV::LooselyCoupledKF::KFMeas, NAV::LooselyCoupledKF::KFMeas, 6, 6>
1847 25517 NAV::LooselyCoupledKF::n_measurementNoiseCovariance_R(const Eigen::Vector3d& gnssVarianceLatLonAlt, const Eigen::Vector3d& gnssVarianceVelocity)
1848 {
1849 // Math: \mathbf{R} = \begin{pmatrix} \sigma^2_\phi & 0 & 0 & 0 & 0 & 0 \\ 0 & \sigma^2_\lambda & 0 & 0 & 0 & 0 \\ 0 & 0 & \sigma^2_h & 0 & 0 & 0 \\ 0 & 0 & 0 & \sigma^2_{v_N} & 0 & 0 \\ 0 & 0 & 0 & 0 & \sigma^2_{v_E} & 0 \\ 0 & 0 & 0 & 0 & 0 & \sigma^2_{v_D} \end{pmatrix}
1850
2/4
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
 25517 KeyedMatrix<double, KFMeas, KFMeas, 6, 6> R(Eigen::Matrix<double, 6, 6>::Zero(), Meas);
1851
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 R.block<3>(dPos, dPos).diagonal() = gnssVarianceLatLonAlt;
1852
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 R.block<3>(dVel, dVel).diagonal() = gnssVarianceVelocity;
1853
1854
5/10
✓ Branch 2 taken 25517 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 25517 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 25517 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 25517 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 25517 times.
✗ Branch 15 not taken.
 102068 R.block<2>({ dPosLat, dPosLon }, { dPosLat, dPosLon }).diagonal() *= std::pow(SCALE_FACTOR_LAT_LON, 2);
1855
1856 25517 return R;
1857 ✗ }
1858
1859 NAV::KeyedMatrix<double, NAV::LooselyCoupledKF::KFMeas, NAV::LooselyCoupledKF::KFMeas, 6, 6>
1860 1296 NAV::LooselyCoupledKF::e_measurementNoiseCovariance_R(const Eigen::Vector3d& gnssVariancePosition, const Eigen::Vector3d& gnssVarianceVelocity)
1861 {
1862 // Math: \mathbf{R} = \begin{pmatrix} \sigma^2_x & 0 & 0 & 0 & 0 & 0 \\ 0 & \sigma^2_y & 0 & 0 & 0 & 0 \\ 0 & 0 & \sigma^2_z & 0 & 0 & 0 \\ 0 & 0 & 0 & \sigma^2_{v_x} & 0 & 0 \\ 0 & 0 & 0 & 0 & \sigma^2_{v_y} & 0 \\ 0 & 0 & 0 & 0 & 0 & \sigma^2_{v_z} \end{pmatrix}
1863
2/4
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1296 times.
✗ Branch 5 not taken.
 1296 KeyedMatrix<double, KFMeas, KFMeas, 6, 6> R(Eigen::Matrix<double, 6, 6>::Zero(), Meas);
1864
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 R.block<3>(dPos, dPos).diagonal() = gnssVariancePosition;
1865
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 R.block<3>(dVel, dVel).diagonal() = gnssVarianceVelocity;
1866
1867 1296 return R;
1868 ✗ }
1869
1870 NAV::KeyedVector<double, NAV::LooselyCoupledKF::KFMeas, 6>
1871 25517 NAV::LooselyCoupledKF::n_measurementInnovation_dz(const Eigen::Vector3d& lla_positionMeasurement, const Eigen::Vector3d& lla_positionEstimate,
1872 const Eigen::Vector3d& n_velocityMeasurement, const Eigen::Vector3d& n_velocityEstimate,
1873 const Eigen::Matrix3d& T_rn_p, const Eigen::Quaterniond& n_Quat_b, const Eigen::Vector3d& b_leverArm_InsGnss,
1874 const Eigen::Vector3d& b_omega_ib, const Eigen::Matrix3d& n_Omega_ie)
1875 {
1876 // 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)
1877
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);
1878
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);
1879
1880
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;
1881
1882
1/2
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
 25517 Eigen::Matrix<double, 6, 1> innovation;
1883
2/4
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
 25517 innovation << deltaLLA, deltaVel;
1884
1885
1/2
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
 51034 return { innovation, Meas };
1886 }
1887
1888 NAV::KeyedVector<double, NAV::LooselyCoupledKF::KFMeas, 6>
1889 1296 NAV::LooselyCoupledKF::e_measurementInnovation_dz(const Eigen::Vector3d& e_positionMeasurement, const Eigen::Vector3d& e_positionEstimate,
1890 const Eigen::Vector3d& e_velocityMeasurement, const Eigen::Vector3d& e_velocityEstimate,
1891 const Eigen::Quaterniond& e_Quat_b, const Eigen::Vector3d& b_leverArm_InsGnss,
1892 const Eigen::Vector3d& b_omega_ib, const Eigen::Matrix3d& e_Omega_ie)
1893 {
1894 // 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)
1895
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;
1896
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);
1897
1898
1/2
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
 1296 Eigen::Matrix<double, 6, 1> innovation;
1899
2/4
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1296 times.
✗ Branch 5 not taken.
 1296 innovation << deltaPos, deltaVel;
1900
1901
1/2
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
 2592 return { innovation, Meas };
1902 }
1903
1904 ✗ std::ostream& operator<<(std::ostream& os, const NAV::LooselyCoupledKF::KFStates& obj)
1905 {
1906 ✗ return os << fmt::format("{}", obj);
1907 }
1908 ✗ std::ostream& operator<<(std::ostream& os, const NAV::LooselyCoupledKF::KFMeas& obj)
1909 {
1910 ✗ return os << fmt::format("{}", obj);
1911 }
1912