0.5.1
Loading...
Searching...
No Matches
TightlyCoupledKF.cpp
Go to the documentation of this file.
1// This file is part of INSTINCT, the INS Toolkit for Integrated
2// Navigation Concepts and Training by the Institute of Navigation of
3// the University of Stuttgart, Germany.
4//
5// This Source Code Form is subject to the terms of the Mozilla Public
6// License, v. 2.0. If a copy of the MPL was not distributed with this
7// file, You can obtain one at https://mozilla.org/MPL/2.0/.
8
9#include "TightlyCoupledKF.hpp"
10
11/*
12
13#include "util/Eigen.hpp"
14#include <cmath>
15
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 "NodeRegistry.hpp"
24#include "Navigation/Constants.hpp"
25#include "Navigation/Ellipsoid/Ellipsoid.hpp"
26#include "Navigation/INS/Functions.hpp"
27#include "Navigation/INS/ProcessNoise.hpp"
28#include "Navigation/INS/EcefFrame/ErrorEquations.hpp"
29#include "Navigation/INS/LocalNavFrame/ErrorEquations.hpp"
30#include "Navigation/GNSS/Positioning/SPP/Algorithm.hpp"
31#include "Navigation/Math/Math.hpp"
32#include "Navigation/Math/VanLoan.hpp"
33#include "Navigation/Gravity/Gravity.hpp"
34#include "Navigation/Transformations/Units.hpp"
35
36#include "Navigation/GNSS/Satellite/internal/SatNavData.hpp"
37#include "Navigation/GNSS/Functions.hpp"
38#include "NodeData/GNSS/GnssNavInfo.hpp"
39#include "NodeData/IMU/ImuObsWDelta.hpp"
40#include "Navigation/GNSS/Satellite/Ephemeris/GLONASSEphemeris.hpp"
41
42#include "util/Logger.hpp"
43#include "util/Container/Vector.hpp"
44
45/// @brief Scale factor to convert the attitude error
46constexpr double SCALE_FACTOR_ATTITUDE = 180. / M_PI;
47/// @brief Scale factor to convert the latitude and longitude error
48constexpr double SCALE_FACTOR_LAT_LON = NAV::InsConst::pseudometre;
49/// @brief Scale factor to convert the acceleration error
50constexpr double SCALE_FACTOR_ACCELERATION = 1e3 / NAV::InsConst::G_NORM;
51/// @brief Scale factor to convert the angular rate error
52constexpr double SCALE_FACTOR_ANGULAR_RATE = 1e3;
53
54NAV::TightlyCoupledKF::TightlyCoupledKF()
55 : Node(typeStatic())
56{
57 LOG_TRACE("{}: called", name);
58
59 _hasConfig = true;
60 _guiConfigDefaultWindowSize = { 866, 938 };
61
62 CreateInputPin(
63 "ImuObs", Pin::Type::Flow, { NAV::ImuObs::type(), NAV::ImuObsWDelta::type() }, &TightlyCoupledKF::recvImuObservation,
64 [](const Node* node, const InputPin& inputPin) {
65 const auto* tckf = static_cast<const TightlyCoupledKF*>(node); // NOLINT(cppcoreguidelines-pro-type-static-cast-downcast)
66 return !inputPin.queue.empty() && tckf->_inertialIntegrator.hasInitialPosition();
67 },
68 1); // Priority 1 ensures, that the IMU obs (prediction) is evaluated before the PosVel obs (update)
69 CreateInputPin(
70 "GnssObs", Pin::Type::Flow, { NAV::GnssObs::type() }, &TightlyCoupledKF::recvGnssObs,
71 [](const Node* node, const InputPin& inputPin) {
72 const auto* tckf = static_cast<const TightlyCoupledKF*>(node); // NOLINT(cppcoreguidelines-pro-type-static-cast-downcast)
73 return !inputPin.queue.empty() && (!tckf->_initializeStateOverExternalPin || tckf->_inertialIntegrator.hasInitialPosition());
74 },
75 2); // Initially this has higher priority than the IMU obs, to initialize the position from it
76
77 updateNumberOfInputPins();
78 CreateOutputPin("PosVelAtt", Pin::Type::Flow, { NAV::InsGnssTCKFSolution::type() });
79}
80
81NAV::TightlyCoupledKF::~TightlyCoupledKF()
82{
83 LOG_TRACE("{}: called", nameId());
84}
85
86std::string NAV::TightlyCoupledKF::typeStatic()
87{
88 return "INS/GNSS TCKF"; // Tightly-coupled Kalman Filter
89}
90
91std::string NAV::TightlyCoupledKF::type() const
92{
93 return typeStatic();
94}
95
96std::string NAV::TightlyCoupledKF::category()
97{
98 return "Data Processor";
99}
100
101void NAV::TightlyCoupledKF::addKalmanMatricesPins()
102{
103 LOG_TRACE("{}: called", nameId());
104
105 if (outputPins.size() == OUTPUT_PORT_INDEX_x)
106 {
107 CreateOutputPin("x", Pin::Type::Matrix, { "Eigen::MatrixXd" }, &_kalmanFilter.x);
108 CreateOutputPin("P", Pin::Type::Matrix, { "Eigen::MatrixXd" }, &_kalmanFilter.P);
109 CreateOutputPin("Phi", Pin::Type::Matrix, { "Eigen::MatrixXd" }, &_kalmanFilter.Phi);
110 CreateOutputPin("Q", Pin::Type::Matrix, { "Eigen::MatrixXd" }, &_kalmanFilter.Q);
111 CreateOutputPin("z", Pin::Type::Matrix, { "Eigen::MatrixXd" }, &_kalmanFilter.z);
112 CreateOutputPin("H", Pin::Type::Matrix, { "Eigen::MatrixXd" }, &_kalmanFilter.H);
113 CreateOutputPin("R", Pin::Type::Matrix, { "Eigen::MatrixXd" }, &_kalmanFilter.R);
114 CreateOutputPin("K", Pin::Type::Matrix, { "Eigen::MatrixXd" }, &_kalmanFilter.K);
115 }
116}
117
118void NAV::TightlyCoupledKF::removeKalmanMatricesPins()
119{
120 LOG_TRACE("{}: called", nameId());
121 while (outputPins.size() > OUTPUT_PORT_INDEX_x)
122 {
123 flow::DeleteOutputPin(outputPins.back());
124 }
125}
126
127void NAV::TightlyCoupledKF::updateExternalPvaInitPin()
128{
129 if (_initializeStateOverExternalPin
130 && inputPins.size() - INPUT_PORT_INDEX_GNSS_NAV_INFO - _nNavInfoPins == 0)
131 {
132 CreateInputPin(
133 "Init PVA", Pin::Type::Flow, { NAV::PosVelAtt::type() }, &TightlyCoupledKF::recvPosVelAttInit,
134 nullptr,
135 3,
136 INPUT_PORT_INDEX_POS_VEL_ATT_INIT);
137 }
138 else if (!_initializeStateOverExternalPin && inputPins.size() - INPUT_PORT_INDEX_GNSS_NAV_INFO - _nNavInfoPins > 0)
139 {
140 flow::DeleteInputPin(inputPins[INPUT_PORT_INDEX_POS_VEL_ATT_INIT]);
141 }
142}
143
144void NAV::TightlyCoupledKF::updateNumberOfInputPins()
145{
146 while (inputPins.size() - static_cast<size_t>(_initializeStateOverExternalPin) - INPUT_PORT_INDEX_GNSS_NAV_INFO < _nNavInfoPins)
147 {
148 CreateInputPin(NAV::GnssNavInfo::type().c_str(), Pin::Type::Object, { NAV::GnssNavInfo::type() });
149 }
150 while (inputPins.size() - static_cast<size_t>(_initializeStateOverExternalPin) - INPUT_PORT_INDEX_GNSS_NAV_INFO > _nNavInfoPins)
151 {
152 flow::DeleteInputPin(inputPins.back());
153 }
154}
155
156void NAV::TightlyCoupledKF::guiConfig()
157{
158 float configWidth = 380.0F * gui::NodeEditorApplication::windowFontRatio();
159 float unitWidth = 150.0F * gui::NodeEditorApplication::windowFontRatio();
160
161 float taylorOrderWidth = 75.0F * gui::NodeEditorApplication::windowFontRatio();
162
163 if (ImGui::CollapsingHeader(fmt::format("Initialization##{}", size_t(id)).c_str(), ImGuiTreeNodeFlags_DefaultOpen))
164 {
165 if (ImGui::Checkbox(fmt::format("Initialize over pin##{}", size_t(id)).c_str(), &_initializeStateOverExternalPin))
166 {
167 updateExternalPvaInitPin();
168 flow::ApplyChanges();
169 }
170 if (!_initializeStateOverExternalPin)
171 {
172 ImGui::SetNextItemWidth(80 * gui::NodeEditorApplication::windowFontRatio());
173 if (ImGui::DragDouble(fmt::format("##initalRollPitchYaw(0) {}", size_t(id)).c_str(),
174 _initalRollPitchYaw.data(), 1.0F, -180.0, 180.0, "%.3f °"))
175 {
176 flow::ApplyChanges();
177 }
178 ImGui::SameLine();
179 ImGui::SetNextItemWidth(80 * gui::NodeEditorApplication::windowFontRatio());
180 if (ImGui::DragDouble(fmt::format("##initalRollPitchYaw(1) {}", size_t(id)).c_str(),
181 &_initalRollPitchYaw[1], 1.0F, -90.0, 90.0, "%.3f °"))
182 {
183 flow::ApplyChanges();
184 }
185 ImGui::SameLine();
186 ImGui::SetNextItemWidth(80 * gui::NodeEditorApplication::windowFontRatio());
187 if (ImGui::DragDouble(fmt::format("##initalRollPitchYaw(2) {}", size_t(id)).c_str(),
188 &_initalRollPitchYaw[2], 1.0, -180.0, 180.0, "%.3f °"))
189 {
190 flow::ApplyChanges();
191 }
192 ImGui::SameLine();
193 ImGui::TextUnformatted("Roll, Pitch, Yaw");
194 }
195 }
196
197 if (ImGui::CollapsingHeader(fmt::format("IMU Integrator settings##{}", size_t(id)).c_str(), ImGuiTreeNodeFlags_DefaultOpen))
198 {
199 if (InertialIntegratorGui(std::to_string(size_t(id)).c_str(), _inertialIntegrator))
200 {
201 flow::ApplyChanges();
202 }
203 if (inputPins.at(INPUT_PORT_INDEX_IMU).isPinLinked()
204 && NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(inputPins.at(INPUT_PORT_INDEX_IMU).link.getConnectedPin()->dataIdentifier, { ImuObsWDelta::type() }))
205 {
206 ImGui::Separator();
207 if (ImGui::Checkbox(fmt::format("Prefer raw measurements over delta##{}", size_t(id)).c_str(), &_preferAccelerationOverDeltaMeasurements))
208 {
209 flow::ApplyChanges();
210 }
211 }
212 }
213
214 ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
215 if (ImGui::CollapsingHeader(fmt::format("Input settings##{}", size_t(id)).c_str()))
216 {
217 ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
218 if (ImGui::TreeNode(fmt::format("Pin settings##{}", size_t(id)).c_str()))
219 {
220 if (ImGui::BeginTable(fmt::format("Pin Settings##{}", size_t(id)).c_str(), inputPins.size() > 1 ? 3 : 2,
221 ImGuiTableFlags_Borders | ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_NoHostExtendX, ImVec2(0.0F, 0.0F)))
222 {
223 ImGui::TableSetupColumn("Pin");
224 ImGui::TableSetupColumn("# Sat");
225 if (inputPins.size() > 3)
226 {
227 ImGui::TableSetupColumn("");
228 }
229 ImGui::TableHeadersRow();
230
231 // Used to reset the member variabel _dragAndDropPinIndex in case no plot does a drag and drop action
232 bool dragAndDropPinStillInProgress = false;
233
234 auto showDragDropTargetPin = [this](size_t pinIdxTarget) {
235 ImGui::Dummy(ImVec2(-1.F, 2.F));
236
237 bool selectableDummy = true;
238 ImGui::PushStyleVar(ImGuiStyleVar_SelectableTextAlign, ImVec2(0.5F, 0.5F));
239 ImGui::PushStyleColor(ImGuiCol_Header, IM_COL32(16, 173, 44, 79));
240 ImGui::Selectable(fmt::format("[drop here]").c_str(), &selectableDummy, ImGuiSelectableFlags_None,
241 ImVec2(std::max(ImGui::GetColumnWidth(0), ImGui::CalcTextSize("[drop here]").x), 20.F));
242 ImGui::PopStyleColor();
243 ImGui::PopStyleVar();
244
245 if (ImGui::BeginDragDropTarget())
246 {
247 if (const ImGuiPayload* payloadData = ImGui::AcceptDragDropPayload(fmt::format("DND Pin {}", size_t(id)).c_str()))
248 {
249 auto pinIdxSource = *static_cast<size_t*>(payloadData->Data);
250
251 if (pinIdxSource < pinIdxTarget)
252 {
253 --pinIdxTarget;
254 }
255
256 move(inputPins, pinIdxSource, pinIdxTarget);
257 flow::ApplyChanges();
258 }
259 ImGui::EndDragDropTarget();
260 }
261 ImGui::Dummy(ImVec2(-1.F, 2.F));
262 };
263
264 for (size_t pinIndex = 0; pinIndex < inputPins.size(); pinIndex++)
265 {
266 ImGui::TableNextRow();
267 ImGui::TableNextColumn(); // Pin
268
269 if (pinIndex == INPUT_PORT_INDEX_GNSS_NAV_INFO && _dragAndDropPinIndex > static_cast<int>(INPUT_PORT_INDEX_GNSS_NAV_INFO))
270 {
271 showDragDropTargetPin(INPUT_PORT_INDEX_GNSS_NAV_INFO);
272 }
273
274 bool selectablePinDummy = false;
275 ImGui::Selectable(fmt::format("{}##{}", inputPins.at(pinIndex).name, size_t(id)).c_str(), &selectablePinDummy);
276 if (pinIndex >= INPUT_PORT_INDEX_GNSS_NAV_INFO && ImGui::BeginDragDropSource(ImGuiDragDropFlags_None))
277 {
278 dragAndDropPinStillInProgress = true;
279 _dragAndDropPinIndex = static_cast<int>(pinIndex);
280 // Data is copied into heap inside the drag and drop
281 ImGui::SetDragDropPayload(fmt::format("DND Pin {}", size_t(id)).c_str(), &pinIndex, sizeof(pinIndex));
282 ImGui::TextUnformatted(inputPins.at(pinIndex).name.c_str());
283 ImGui::EndDragDropSource();
284 }
285 if (_dragAndDropPinIndex > 0 && pinIndex >= INPUT_PORT_INDEX_GNSS_NAV_INFO
286 && pinIndex != static_cast<size_t>(_dragAndDropPinIndex - 1)
287 && pinIndex != static_cast<size_t>(_dragAndDropPinIndex))
288 {
289 showDragDropTargetPin(pinIndex + 1);
290 }
291 if (pinIndex >= INPUT_PORT_INDEX_GNSS_NAV_INFO && ImGui::IsItemHovered())
292 {
293 ImGui::SetTooltip("This item can be dragged to reorder the pins");
294 }
295
296 ImGui::TableNextColumn(); // # Sat
297 if (auto gnssNavInfo = getInputValue<GnssNavInfo>(pinIndex))
298 {
299 size_t usedSatNum = 0;
300 std::string usedSats;
301 std::string allSats;
302
303 std::string filler = ", ";
304 for (const auto& satellite : gnssNavInfo->v->satellites())
305 {
306 if ((satellite.first.satSys & _filterFreq)
307 && std::ranges::find(_excludedSatellites, satellite.first) == _excludedSatellites.end())
308 {
309 usedSatNum++;
310 usedSats += (allSats.empty() ? "" : filler) + fmt::format("{}", satellite.first);
311 }
312 allSats += (allSats.empty() ? "" : filler) + fmt::format("{}", satellite.first);
313 }
314 ImGui::TextUnformatted(fmt::format("{} / {}", usedSatNum, gnssNavInfo->v->nSatellites()).c_str());
315 if (ImGui::IsItemHovered())
316 {
317 ImGui::SetTooltip("Used satellites: %s\n"
318 "All satellites: %s",
319 usedSats.c_str(), allSats.c_str());
320 }
321 }
322
323 if (inputPins.size() > 2)
324 {
325 ImGui::TableNextColumn(); // Delete
326 if (ImGui::Button(fmt::format("x##{} - {}", size_t(id), pinIndex).c_str()))
327 {
328 _nNavInfoPins--;
329 flow::DeleteInputPin(inputPins.at(pinIndex));
330 flow::ApplyChanges();
331 }
332 if (ImGui::IsItemHovered())
333 {
334 ImGui::SetTooltip("Delete the pin");
335 }
336 }
337 }
338
339 if (!dragAndDropPinStillInProgress)
340 {
341 _dragAndDropPinIndex = -1;
342 }
343
344 ImGui::TableNextRow();
345 ImGui::TableNextColumn(); // Pin
346 if (ImGui::Button(fmt::format("Add Pin##{}", size_t(id)).c_str()))
347 {
348 _nNavInfoPins++;
349 LOG_DEBUG("{}: # Input Pins changed to {}", nameId(), _nNavInfoPins);
350 flow::ApplyChanges();
351 updateNumberOfInputPins();
352 }
353
354 ImGui::EndTable();
355 }
356
357 ImGui::TreePop();
358 }
359
360 ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
361 if (ImGui::TreeNode(fmt::format("GNSS input settings##{}", size_t(id)).c_str()))
362 {
363 ImGui::BeginDisabled();
364 ImGui::SetNextItemWidth(configWidth);
365 if (ShowFrequencySelector(fmt::format("Satellite Frequencies##{}", size_t(id)).c_str(), _filterFreq))
366 {
367 flow::ApplyChanges();
368 }
369 ImGui::EndDisabled();
370
371 ImGui::SetNextItemWidth(configWidth);
372 if (ShowCodeSelector(fmt::format("Signal Codes##{}", size_t(id)).c_str(), _filterCode, _filterFreq))
373 {
374 flow::ApplyChanges();
375 }
376
377 ImGui::SetNextItemWidth(configWidth);
378 if (ShowSatelliteSelector(fmt::format("Excluded satellites##{}", size_t(id)).c_str(), _excludedSatellites))
379 {
380 flow::ApplyChanges();
381 }
382
383 double elevationMaskDeg = rad2deg(_elevationMask);
384 ImGui::SetNextItemWidth(configWidth);
385 if (ImGui::InputDoubleL(fmt::format("Elevation mask##{}", size_t(id)).c_str(), &elevationMaskDeg, 0.0, 90.0, 5.0, 5.0, "%.1f°", ImGuiInputTextFlags_AllowTabInput))
386 {
387 _elevationMask = deg2rad(elevationMaskDeg);
388 LOG_DEBUG("{}: Elevation mask changed to {}°", nameId(), elevationMaskDeg);
389 flow::ApplyChanges();
390 }
391
392 ImGui::TreePop();
393 }
394
395 ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
396 if (ImGui::TreeNode(fmt::format("Compensation models##GNSS {}", size_t(id)).c_str()))
397 {
398 ImGui::SetNextItemWidth(configWidth - ImGui::GetStyle().IndentSpacing);
399 if (ComboIonosphereModel(fmt::format("Ionosphere Model##{}", size_t(id)).c_str(), _ionosphereModel))
400 {
401 LOG_DEBUG("{}: Ionosphere Model changed to {}", nameId(), NAV::to_string(_ionosphereModel));
402 flow::ApplyChanges();
403 }
404 if (ComboTroposphereModel(fmt::format("Troposphere Model##{}", size_t(id)).c_str(), _troposphereModels, configWidth - ImGui::GetStyle().IndentSpacing))
405 {
406 flow::ApplyChanges();
407 }
408 ImGui::TreePop();
409 }
410 }
411
412 ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
413 if (ImGui::CollapsingHeader(fmt::format("Kalman Filter settings##{}", size_t(id)).c_str()))
414 {
415 if (_phiCalculationAlgorithm == PhiCalculationAlgorithm::Taylor)
416 {
417 ImGui::SetNextItemWidth(configWidth - taylorOrderWidth);
418 }
419 else
420 {
421 ImGui::SetNextItemWidth(configWidth + ImGui::GetStyle().ItemSpacing.x);
422 }
423 if (auto phiCalculationAlgorithm = static_cast<int>(_phiCalculationAlgorithm);
424 ImGui::Combo(fmt::format("##Phi calculation algorithm {}", size_t(id)).c_str(), &phiCalculationAlgorithm, "Van Loan\0Taylor\0\0"))
425 {
426 _phiCalculationAlgorithm = static_cast<decltype(_phiCalculationAlgorithm)>(phiCalculationAlgorithm);
427 LOG_DEBUG("{}: Phi calculation algorithm changed to {}", nameId(), fmt::underlying(_phiCalculationAlgorithm));
428 flow::ApplyChanges();
429 }
430
431 if (_phiCalculationAlgorithm == PhiCalculationAlgorithm::Taylor)
432 {
433 ImGui::SameLine();
434 ImGui::SetNextItemWidth(taylorOrderWidth);
435 if (ImGui::InputIntL(fmt::format("##Phi calculation Taylor Order {}", size_t(id)).c_str(), &_phiCalculationTaylorOrder, 1, 9))
436 {
437 LOG_DEBUG("{}: Phi calculation Taylor Order changed to {}", nameId(), _phiCalculationTaylorOrder);
438 flow::ApplyChanges();
439 }
440 }
441 ImGui::SameLine();
442 ImGui::SetCursorPosX(ImGui::GetCursorPosX() - ImGui::GetStyle().ItemSpacing.x + ImGui::GetStyle().ItemInnerSpacing.x);
443 ImGui::Text("Phi calculation algorithm%s", _phiCalculationAlgorithm == PhiCalculationAlgorithm::Taylor ? " (up to order)" : "");
444
445 ImGui::SetNextItemWidth(configWidth + ImGui::GetStyle().ItemSpacing.x);
446 if (auto qCalculationAlgorithm = static_cast<int>(_qCalculationAlgorithm);
447 ImGui::Combo(fmt::format("Q calculation algorithm##{}", size_t(id)).c_str(), &qCalculationAlgorithm, "Van Loan\0Taylor 1st Order (Groves 2013)\0\0"))
448 {
449 _qCalculationAlgorithm = static_cast<decltype(_qCalculationAlgorithm)>(qCalculationAlgorithm);
450 LOG_DEBUG("{}: Q calculation algorithm changed to {}", nameId(), fmt::underlying(_qCalculationAlgorithm));
451 flow::ApplyChanges();
452 }
453
454 // ###########################################################################################################
455 // Q - System/Process noise covariance matrix
456 // ###########################################################################################################
457
458 ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
459 if (ImGui::TreeNode(fmt::format("Q - System/Process noise covariance matrix##{}", size_t(id)).c_str()))
460 {
461 // --------------------------------------------- Accelerometer -----------------------------------------------
462
463 if (_qCalculationAlgorithm == QCalculationAlgorithm::VanLoan)
464 {
465 ImGui::SetNextItemWidth(configWidth + ImGui::GetStyle().ItemSpacing.x);
466 if (auto randomProcessAccel = static_cast<int>(_randomProcessAccel);
467 ImGui::Combo(fmt::format("Random Process Accelerometer##{}", size_t(id)).c_str(), &randomProcessAccel, "Random Walk\0"
468 "Gauss-Markov 1st Order\0\0"))
469 {
470 _randomProcessAccel = static_cast<decltype(_randomProcessAccel)>(randomProcessAccel);
471 LOG_DEBUG("{}: randomProcessAccel changed to {}", nameId(), fmt::underlying(_randomProcessAccel));
472 flow::ApplyChanges();
473 }
474 }
475
476 if (gui::widgets::InputDouble3WithUnit(fmt::format("Standard deviation of the noise on the\naccelerometer specific-force measurements##{}", size_t(id)).c_str(),
477 configWidth, unitWidth, _stdev_ra.data(), _stdevAccelNoiseUnits, "mg/√(Hz)\0m/s^2/√(Hz)\0\0",
478 "%.2e", ImGuiInputTextFlags_CharsScientific))
479 {
480 LOG_DEBUG("{}: stdev_ra changed to {}", nameId(), _stdev_ra.transpose());
481 LOG_DEBUG("{}: stdevAccelNoiseUnits changed to {}", nameId(), fmt::underlying(_stdevAccelNoiseUnits));
482 flow::ApplyChanges();
483 }
484 if (gui::widgets::InputDouble3WithUnit(fmt::format("Standard deviation σ of the accel {}##{}",
485 _qCalculationAlgorithm == QCalculationAlgorithm::Taylor1
486 ? "dynamic bias, in σ²/τ"
487 : (_randomProcessAccel == RandomProcess::RandomWalk ? "bias noise" : "bias noise, in √(2σ²β)"),
488 size_t(id))
489 .c_str(),
490 configWidth, unitWidth, _stdev_bad.data(), _stdevAccelBiasUnits, "µg\0m/s^2\0\0",
491 "%.2e", ImGuiInputTextFlags_CharsScientific))
492 {
493 LOG_DEBUG("{}: stdev_bad changed to {}", nameId(), _stdev_bad.transpose());
494 LOG_DEBUG("{}: stdevAccelBiasUnits changed to {}", nameId(), fmt::underlying(_stdevAccelBiasUnits));
495 flow::ApplyChanges();
496 }
497
498 if (_randomProcessAccel == RandomProcess::GaussMarkov1 || _qCalculationAlgorithm == QCalculationAlgorithm::Taylor1)
499 {
500 ImGui::SetNextItemWidth(configWidth - unitWidth);
501 if (ImGui::InputDouble3L(fmt::format("##Correlation length τ of the accel dynamic bias {}", size_t(id)).c_str(), _tau_bad.data(), 0., std::numeric_limits<double>::max(), "%.2e", ImGuiInputTextFlags_CharsScientific))
502 {
503 LOG_DEBUG("{}: tau_bad changed to {}", nameId(), _tau_bad);
504 flow::ApplyChanges();
505 }
506 ImGui::SameLine();
507 int unitCorrelationLength = 0;
508 ImGui::SetNextItemWidth(unitWidth);
509 ImGui::Combo(fmt::format("##Correlation length of the accel dynamic bias unit {}", size_t(id)).c_str(), &unitCorrelationLength, "s\0\0");
510 ImGui::SameLine();
511 ImGui::SetCursorPosX(ImGui::GetCursorPosX() - ImGui::GetStyle().ItemSpacing.x + ImGui::GetStyle().ItemInnerSpacing.x);
512 if (_qCalculationAlgorithm == QCalculationAlgorithm::VanLoan)
513 {
514 ImGui::TextUnformatted("Correlation length τ of the accel bias noise");
515 }
516 else if (_qCalculationAlgorithm == QCalculationAlgorithm::Taylor1)
517 {
518 ImGui::TextUnformatted("Correlation length τ of the accel dynamic bias");
519 }
520 }
521
522 ImGui::SetCursorPosY(ImGui::GetCursorPosY() + 20.F);
523
524 // ----------------------------------------------- Gyroscope -------------------------------------------------
525
526 if (_qCalculationAlgorithm == QCalculationAlgorithm::VanLoan)
527 {
528 ImGui::SetNextItemWidth(configWidth + ImGui::GetStyle().ItemSpacing.x);
529 if (auto randomProcessGyro = static_cast<int>(_randomProcessGyro);
530 ImGui::Combo(fmt::format("Random Process Gyroscope##{}", size_t(id)).c_str(), &randomProcessGyro, "Random Walk\0"
531 "Gauss-Markov 1st Order\0\0"))
532 {
533 _randomProcessGyro = static_cast<decltype(_randomProcessGyro)>(randomProcessGyro);
534 LOG_DEBUG("{}: randomProcessGyro changed to {}", nameId(), fmt::underlying(_randomProcessGyro));
535 flow::ApplyChanges();
536 }
537 }
538
539 if (gui::widgets::InputDouble3WithUnit(fmt::format("Standard deviation of the noise on\nthe gyro angular-rate measurements##{}", size_t(id)).c_str(),
540 configWidth, unitWidth, _stdev_rg.data(), _stdevGyroNoiseUnits, "deg/hr/√(Hz)\0rad/s/√(Hz)\0\0",
541 "%.2e", ImGuiInputTextFlags_CharsScientific))
542 {
543 LOG_DEBUG("{}: stdev_rg changed to {}", nameId(), _stdev_rg.transpose());
544 LOG_DEBUG("{}: stdevGyroNoiseUnits changed to {}", nameId(), fmt::underlying(_stdevGyroNoiseUnits));
545 flow::ApplyChanges();
546 }
547 if (gui::widgets::InputDouble3WithUnit(fmt::format("Standard deviation σ of the gyro {}##{}",
548 _qCalculationAlgorithm == QCalculationAlgorithm::Taylor1
549 ? "dynamic bias, in σ²/τ"
550 : (_randomProcessGyro == RandomProcess::RandomWalk ? "bias noise" : "bias noise, in √(2σ²β)"),
551 size_t(id))
552 .c_str(),
553 configWidth, unitWidth, _stdev_bgd.data(), _stdevGyroBiasUnits, "°/h\0rad/s\0\0",
554 "%.2e", ImGuiInputTextFlags_CharsScientific))
555 {
556 LOG_DEBUG("{}: stdev_bgd changed to {}", nameId(), _stdev_bgd.transpose());
557 LOG_DEBUG("{}: stdevGyroBiasUnits changed to {}", nameId(), fmt::underlying(_stdevGyroBiasUnits));
558 flow::ApplyChanges();
559 }
560
561 if (_randomProcessGyro == RandomProcess::GaussMarkov1 || _qCalculationAlgorithm == QCalculationAlgorithm::Taylor1)
562 {
563 ImGui::SetNextItemWidth(configWidth - unitWidth);
564 if (ImGui::InputDouble3L(fmt::format("##Correlation length τ of the gyro dynamic bias {}", size_t(id)).c_str(), _tau_bgd.data(), 0., std::numeric_limits<double>::max(), "%.2e", ImGuiInputTextFlags_CharsScientific))
565 {
566 LOG_DEBUG("{}: tau_bgd changed to {}", nameId(), _tau_bgd);
567 flow::ApplyChanges();
568 }
569 ImGui::SameLine();
570 int unitCorrelationLength = 0;
571 ImGui::SetNextItemWidth(unitWidth);
572 ImGui::Combo(fmt::format("##Correlation length of the gyro dynamic bias unit {}", size_t(id)).c_str(), &unitCorrelationLength, "s\0\0");
573 ImGui::SameLine();
574 ImGui::SetCursorPosX(ImGui::GetCursorPosX() - ImGui::GetStyle().ItemSpacing.x + ImGui::GetStyle().ItemInnerSpacing.x);
575 if (_qCalculationAlgorithm == QCalculationAlgorithm::VanLoan)
576 {
577 ImGui::TextUnformatted("Correlation length τ of the gyro bias noise");
578 }
579 else if (_qCalculationAlgorithm == QCalculationAlgorithm::Taylor1)
580 {
581 ImGui::TextUnformatted("Correlation length τ of the gyro dynamic bias");
582 }
583 }
584
585 // --------------------------------------------- Clock -----------------------------------------------
586
587 if (gui::widgets::InputDoubleWithUnit(fmt::format("Standard deviation of the receiver\nclock phase drift (RW)##{}", size_t(id)).c_str(),
588 configWidth, unitWidth, &_stdev_cp, _stdevClockPhaseUnits, "m/√(Hz)\0\0",
589 0.0, 0.0, "%.2e", ImGuiInputTextFlags_CharsScientific))
590 {
591 LOG_DEBUG("{}: stdev_cf changed to {}", nameId(), _stdev_cp);
592 LOG_DEBUG("{}: stdevClockPhaseUnits changed to {}", nameId(), fmt::underlying(_stdevClockPhaseUnits));
593 flow::ApplyChanges();
594 }
595
596 if (gui::widgets::InputDoubleWithUnit(fmt::format("Standard deviation of the receiver\nclock frequency drift (IRW)##{}", size_t(id)).c_str(),
597 configWidth, unitWidth, &_stdev_cf, _stdevClockFreqUnits, "m/s/√(Hz)\0\0",
598 0.0, 0.0, "%.2e", ImGuiInputTextFlags_CharsScientific))
599 {
600 LOG_DEBUG("{}: stdev_cf changed to {}", nameId(), _stdev_cf);
601 LOG_DEBUG("{}: stdevClockFreqUnits changed to {}", nameId(), fmt::underlying(_stdevClockFreqUnits));
602 flow::ApplyChanges();
603 }
604
605 ImGui::TreePop();
606 }
607
608 // ###########################################################################################################
609 // Measurement Uncertainties 𝐑
610 // ###########################################################################################################
611
612 // TODO: Replace with GNSS Measurement Error Model (see SPP node)
613 // ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
614 // if (ImGui::TreeNode(fmt::format("R - Measurement noise covariance matrix##{}", size_t(id)).c_str()))
615 // {
616 // if (gui::widgets::InputDoubleWithUnit(fmt::format("Pseudorange covariance ({})##{}",
617 // _gnssMeasurementUncertaintyPseudorangeUnit == GnssMeasurementUncertaintyPseudorangeUnit::meter2
618 // ? "Variance σ²"
619 // : "Standard deviation σ",
620 // size_t(id))
621 // .c_str(),
622 // configWidth, unitWidth, &_gnssMeasurementUncertaintyPseudorange, _gnssMeasurementUncertaintyPseudorangeUnit, "m^2\0"
623 // "m\0",
624 // 0.0, 0.0, "%.2e", ImGuiInputTextFlags_CharsScientific))
625 // {
626 // LOG_DEBUG("{}: gnssMeasurementUncertaintyPseudorange changed to {}", nameId(), _gnssMeasurementUncertaintyPseudorange);
627 // LOG_DEBUG("{}: gnssMeasurementUncertaintyPseudorangeUnit changed to {}", nameId(), fmt::underlying(_gnssMeasurementUncertaintyPseudorangeUnit));
628 // flow::ApplyChanges();
629 // }
630
631 // if (gui::widgets::InputDoubleWithUnit(fmt::format("Pseudorange-rate covariance ({})##{}",
632 // _gnssMeasurementUncertaintyPseudorangeRateUnit == GnssMeasurementUncertaintyPseudorangeRateUnit::m2_s2
633 // ? "Variance σ²"
634 // : "Standard deviation σ",
635 // size_t(id))
636 // .c_str(),
637 // configWidth, unitWidth, &_gnssMeasurementUncertaintyPseudorangeRate, _gnssMeasurementUncertaintyPseudorangeRateUnit, "m^2/s^2\0"
638 // "m/s\0",
639 // 0.0, 0.0, "%.2e", ImGuiInputTextFlags_CharsScientific))
640 // {
641 // LOG_DEBUG("{}: gnssMeasurementUncertaintyPseudorangeRate changed to {}", nameId(), _gnssMeasurementUncertaintyPseudorangeRate);
642 // LOG_DEBUG("{}: gnssMeasurementUncertaintyPseudorangeRateUnit changed to {}", nameId(), fmt::underlying(_gnssMeasurementUncertaintyPseudorangeRateUnit));
643 // flow::ApplyChanges();
644 // }
645
646 // ImGui::TreePop();
647 // }
648
649 // ###########################################################################################################
650 // 𝐏 Error covariance matrix
651 // ###########################################################################################################
652
653 ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
654 if (ImGui::TreeNode(fmt::format("P - Error covariance matrix (init)##{}", size_t(id)).c_str()))
655 {
656 if (gui::widgets::InputDouble3WithUnit(fmt::format("Position covariance ({})##{}",
657 _initCovariancePositionUnit == InitCovariancePositionUnit::rad2_rad2_m2
658 || _initCovariancePositionUnit == InitCovariancePositionUnit::meter2
659 ? "Variance σ²"
660 : "Standard deviation σ",
661 size_t(id))
662 .c_str(),
663 configWidth, unitWidth, _initCovariancePosition.data(), _initCovariancePositionUnit, "rad^2, rad^2, m^2\0"
664 "rad, rad, m\0"
665 "m^2, m^2, m^2\0"
666 "m, m, m\0\0",
667 "%.2e", ImGuiInputTextFlags_CharsScientific))
668 {
669 LOG_DEBUG("{}: initCovariancePosition changed to {}", nameId(), _initCovariancePosition);
670 LOG_DEBUG("{}: initCovariancePositionUnit changed to {}", nameId(), fmt::underlying(_initCovariancePositionUnit));
671 flow::ApplyChanges();
672 }
673
674 if (gui::widgets::InputDouble3WithUnit(fmt::format("Velocity covariance ({})##{}",
675 _initCovarianceVelocityUnit == InitCovarianceVelocityUnit::m2_s2
676 ? "Variance σ²"
677 : "Standard deviation σ",
678 size_t(id))
679 .c_str(),
680 configWidth, unitWidth, _initCovarianceVelocity.data(), _initCovarianceVelocityUnit, "m^2/s^2\0"
681 "m/s\0\0",
682 "%.2e", ImGuiInputTextFlags_CharsScientific))
683 {
684 LOG_DEBUG("{}: initCovarianceVelocity changed to {}", nameId(), _initCovarianceVelocity);
685 LOG_DEBUG("{}: initCovarianceVelocityUnit changed to {}", nameId(), fmt::underlying(_initCovarianceVelocityUnit));
686 flow::ApplyChanges();
687 }
688
689 if (gui::widgets::InputDouble3WithUnit(fmt::format("Flight Angles covariance ({})##{}",
690 _initCovarianceAttitudeAnglesUnit == InitCovarianceAttitudeAnglesUnit::rad2
691 || _initCovarianceAttitudeAnglesUnit == InitCovarianceAttitudeAnglesUnit::deg2
692 ? "Variance σ²"
693 : "Standard deviation σ",
694 size_t(id))
695 .c_str(),
696 configWidth, unitWidth, _initCovarianceAttitudeAngles.data(), _initCovarianceAttitudeAnglesUnit, "rad^2\0"
697 "deg^2\0"
698 "rad\0"
699 "deg\0\0",
700 "%.2e", ImGuiInputTextFlags_CharsScientific))
701 {
702 LOG_DEBUG("{}: initCovarianceAttitudeAngles changed to {}", nameId(), _initCovarianceAttitudeAngles);
703 LOG_DEBUG("{}: initCovarianceAttitudeAnglesUnit changed to {}", nameId(), fmt::underlying(_initCovarianceAttitudeAnglesUnit));
704 flow::ApplyChanges();
705 }
706 ImGui::SameLine();
707 gui::widgets::HelpMarker(_inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::ECEF
708 ? "Angles rotating the ECEF frame into the body frame."
709 : "Angles rotating the local navigation frame into the body frame (Roll, Pitch, Yaw)");
710
711 if (gui::widgets::InputDouble3WithUnit(fmt::format("Accelerometer Bias covariance ({})##{}",
712 _initCovarianceBiasAccelUnit == InitCovarianceBiasAccelUnit::m2_s4
713 ? "Variance σ²"
714 : "Standard deviation σ",
715 size_t(id))
716 .c_str(),
717 configWidth, unitWidth, _initCovarianceBiasAccel.data(), _initCovarianceBiasAccelUnit, "m^2/s^4\0"
718 "m/s^2\0\0",
719 "%.2e", ImGuiInputTextFlags_CharsScientific))
720 {
721 LOG_DEBUG("{}: initCovarianceBiasAccel changed to {}", nameId(), _initCovarianceBiasAccel);
722 LOG_DEBUG("{}: initCovarianceBiasAccelUnit changed to {}", nameId(), fmt::underlying(_initCovarianceBiasAccelUnit));
723 flow::ApplyChanges();
724 }
725
726 if (gui::widgets::InputDouble3WithUnit(fmt::format("Gyroscope Bias covariance ({})##{}",
727 _initCovarianceBiasGyroUnit == InitCovarianceBiasGyroUnit::rad2_s2
728 || _initCovarianceBiasGyroUnit == InitCovarianceBiasGyroUnit::deg2_s2
729 ? "Variance σ²"
730 : "Standard deviation σ",
731 size_t(id))
732 .c_str(),
733 configWidth, unitWidth, _initCovarianceBiasGyro.data(), _initCovarianceBiasGyroUnit, "rad^2/s^2\0"
734 "deg^2/s^2\0"
735 "rad/s\0"
736 "deg/s\0\0",
737 "%.2e", ImGuiInputTextFlags_CharsScientific))
738 {
739 LOG_DEBUG("{}: initCovarianceBiasGyro changed to {}", nameId(), _initCovarianceBiasGyro);
740 LOG_DEBUG("{}: initCovarianceBiasGyroUnit changed to {}", nameId(), fmt::underlying(_initCovarianceBiasGyroUnit));
741 flow::ApplyChanges();
742 }
743
744 if (gui::widgets::InputDoubleWithUnit(fmt::format("Receiver clock phase drift covariance ({})##{}",
745 _initCovariancePhaseUnit == InitCovarianceClockPhaseUnit::m2
746 || _initCovariancePhaseUnit == InitCovarianceClockPhaseUnit::s2
747 ? "Variance σ²"
748 : "Standard deviation σ",
749 size_t(id))
750 .c_str(),
751 configWidth, unitWidth, &_initCovariancePhase, _initCovariancePhaseUnit, "m^2\0"
752 "s^2\0"
753 "m\0"
754 "s\0\0",
755 0.0, 0.0, "%.2e", ImGuiInputTextFlags_CharsScientific))
756 {
757 LOG_DEBUG("{}: initCovariancePhase changed to {}", nameId(), _initCovariancePhase);
758 LOG_DEBUG("{}: InitCovarianceClockPhaseUnit changed to {}", nameId(), fmt::underlying(_initCovariancePhaseUnit));
759 flow::ApplyChanges();
760 }
761
762 if (gui::widgets::InputDoubleWithUnit(fmt::format("Receiver clock frequency drift covariance ({})##{}",
763 _initCovarianceFreqUnit == InitCovarianceClockFreqUnit::m2_s2
764 ? "Variance σ²"
765 : "Standard deviation σ",
766 size_t(id))
767 .c_str(),
768 configWidth, unitWidth, &_initCovarianceFreq, _initCovarianceFreqUnit, "m^2/s^2\0"
769 "m/s\0\0",
770 0.0, 0.0, "%.2e", ImGuiInputTextFlags_CharsScientific))
771 {
772 LOG_DEBUG("{}: initCovarianceFreq changed to {}", nameId(), _initCovarianceFreq);
773 LOG_DEBUG("{}: initCovarianceFreqUnit changed to {}", nameId(), fmt::underlying(_initCovarianceFreqUnit));
774 flow::ApplyChanges();
775 }
776
777 ImGui::TreePop();
778 }
779
780 // ###########################################################################################################
781 // IMU biases
782 // ###########################################################################################################
783
784 ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
785 if (ImGui::TreeNode(fmt::format("IMU biases (init)##{}", size_t(id)).c_str()))
786 {
787 if (gui::widgets::InputDouble3WithUnit(fmt::format("Accelerometer biases##{}", size_t(id)).c_str(),
788 configWidth, unitWidth, _initBiasAccel.data(), _initBiasAccelUnit, "m/s^2\0\0",
789 "%.2e", ImGuiInputTextFlags_CharsScientific))
790 {
791 LOG_DEBUG("{}: initBiasAccel changed to {}", nameId(), _initBiasAccel.transpose());
792 LOG_DEBUG("{}: initBiasAccelUnit changed to {}", nameId(), fmt::underlying(_initBiasAccelUnit));
793 flow::ApplyChanges();
794 }
795 if (gui::widgets::InputDouble3WithUnit(fmt::format("Gyro biases##{}", size_t(id)).c_str(),
796 configWidth, unitWidth, _initBiasGyro.data(), _initBiasGyroUnit, "rad/s\0deg/s\0\0",
797 "%.2e", ImGuiInputTextFlags_CharsScientific))
798 {
799 LOG_DEBUG("{}: initBiasGyro changed to {}", nameId(), _initBiasGyro.transpose());
800 LOG_DEBUG("{}: initBiasGyroUnit changed to {}", nameId(), fmt::underlying(_initBiasGyroUnit));
801 flow::ApplyChanges();
802 }
803
804 ImGui::TreePop();
805 }
806
807 ImGui::Separator();
808
809 if (ImGui::Checkbox(fmt::format("Rank check for Kalman filter matrices##{}", size_t(id)).c_str(), &_checkKalmanMatricesRanks))
810 {
811 LOG_DEBUG("{}: checkKalmanMatricesRanks {}", nameId(), _checkKalmanMatricesRanks);
812 flow::ApplyChanges();
813 }
814
815 if (ImGui::Checkbox(fmt::format("Show Kalman Filter matrices as output pins##{}", size_t(id)).c_str(), &_showKalmanFilterOutputPins))
816 {
817 LOG_DEBUG("{}: showKalmanFilterOutputPins {}", nameId(), _showKalmanFilterOutputPins);
818 if (_showKalmanFilterOutputPins)
819 {
820 addKalmanMatricesPins();
821 }
822 else
823 {
824 removeKalmanMatricesPins();
825 }
826 flow::ApplyChanges();
827 }
828 }
829}
830
831[[nodiscard]] json NAV::TightlyCoupledKF::save() const
832{
833 LOG_TRACE("{}: called", nameId());
834
835 json j;
836
837 j["inertialIntegrator"] = _inertialIntegrator;
838 j["showKalmanFilterOutputPins"] = _showKalmanFilterOutputPins;
839 j["nNavInfoPins"] = _nNavInfoPins;
840 j["frequencies"] = Frequency_(_filterFreq);
841 j["codes"] = _filterCode;
842 j["excludedSatellites"] = _excludedSatellites;
843 j["elevationMask"] = rad2deg(_elevationMask);
844 j["ionosphereModel"] = _ionosphereModel;
845 j["troposphereModels"] = _troposphereModels;
846
847 j["checkKalmanMatricesRanks"] = _checkKalmanMatricesRanks;
848 j["phiCalculationAlgorithm"] = _phiCalculationAlgorithm;
849 j["phiCalculationTaylorOrder"] = _phiCalculationTaylorOrder;
850 j["qCalculationAlgorithm"] = _qCalculationAlgorithm;
851
852 j["randomProcessAccel"] = _randomProcessAccel;
853 j["randomProcessGyro"] = _randomProcessGyro;
854 j["stdev_ra"] = _stdev_ra;
855 j["stdevAccelNoiseUnits"] = _stdevAccelNoiseUnits;
856 j["stdev_rg"] = _stdev_rg;
857 j["stdevGyroNoiseUnits"] = _stdevGyroNoiseUnits;
858 j["stdev_bad"] = _stdev_bad;
859 j["tau_bad"] = _tau_bad;
860 j["stdevAccelBiasUnits"] = _stdevAccelBiasUnits;
861 j["stdev_bgd"] = _stdev_bgd;
862 j["tau_bgd"] = _tau_bgd;
863 j["stdevGyroBiasUnits"] = _stdevGyroBiasUnits;
864 j["stdevClockFreqUnits"] = _stdevClockFreqUnits;
865 j["stdev_cp"] = _stdev_cp;
866 j["stdev_cf"] = _stdev_cf;
867 j["initBiasAccel"] = _initBiasAccel;
868 j["initBiasAccelUnit"] = _initBiasAccelUnit;
869 j["initBiasGyro"] = _initBiasGyro;
870 j["initBiasGyroUnit"] = _initBiasGyroUnit;
871
872 j["initCovariancePositionUnit"] = _initCovariancePositionUnit;
873 j["initCovariancePosition"] = _initCovariancePosition;
874 j["initCovarianceVelocityUnit"] = _initCovarianceVelocityUnit;
875 j["initCovarianceVelocity"] = _initCovarianceVelocity;
876 j["initCovarianceAttitudeAnglesUnit"] = _initCovarianceAttitudeAnglesUnit;
877 j["initCovarianceAttitudeAngles"] = _initCovarianceAttitudeAngles;
878 j["initCovarianceBiasAccelUnit"] = _initCovarianceBiasAccelUnit;
879 j["initCovarianceBiasAccel"] = _initCovarianceBiasAccel;
880 j["initCovarianceBiasGyroUnit"] = _initCovarianceBiasGyroUnit;
881 j["initCovarianceBiasGyro"] = _initCovarianceBiasGyro;
882 j["initCovariancePhaseUnit"] = _initCovariancePhaseUnit;
883 j["initCovariancePhase"] = _initCovariancePhase;
884 j["initCovarianceFreqUnit"] = _initCovarianceFreqUnit;
885 j["initCovarianceFreq"] = _initCovarianceFreq;
886 // j["gnssMeasurementUncertaintyPseudorangeUnit"] = _gnssMeasurementUncertaintyPseudorangeUnit; // TODO: Replace with GNSS Measurement Error Model (see SPP node)
887 // j["gnssMeasurementUncertaintyPseudorange"] = _gnssMeasurementUncertaintyPseudorange;
888 // j["gnssMeasurementUncertaintyPseudorangeRateUnit"] = _gnssMeasurementUncertaintyPseudorangeRateUnit;
889 // j["gnssMeasurementUncertaintyPseudorangeRate"] = _gnssMeasurementUncertaintyPseudorangeRate;
890
891 return j;
892}
893
894void NAV::TightlyCoupledKF::restore(json const& j)
895{
896 LOG_TRACE("{}: called", nameId());
897
898 if (j.contains("inertialIntegrator"))
899 {
900 j.at("inertialIntegrator").get_to(_inertialIntegrator);
901 }
902 if (j.contains("showKalmanFilterOutputPins"))
903 {
904 j.at("showKalmanFilterOutputPins").get_to(_showKalmanFilterOutputPins);
905 if (_showKalmanFilterOutputPins)
906 {
907 addKalmanMatricesPins();
908 }
909 }
910 if (j.contains("nNavInfoPins"))
911 {
912 j.at("nNavInfoPins").get_to(_nNavInfoPins);
913 updateNumberOfInputPins();
914 }
915 if (j.contains("frequencies"))
916 {
917 uint64_t value = 0;
918 j.at("frequencies").get_to(value);
919 _filterFreq = Frequency_(value);
920 }
921 if (j.contains("codes"))
922 {
923 j.at("codes").get_to(_filterCode);
924 }
925 if (j.contains("excludedSatellites"))
926 {
927 j.at("excludedSatellites").get_to(_excludedSatellites);
928 }
929 if (j.contains("elevationMask"))
930 {
931 j.at("elevationMask").get_to(_elevationMask);
932 _elevationMask = deg2rad(_elevationMask);
933 }
934 if (j.contains("ionosphereModel"))
935 {
936 j.at("ionosphereModel").get_to(_ionosphereModel);
937 }
938 if (j.contains("troposphereModels"))
939 {
940 j.at("troposphereModels").get_to(_troposphereModels);
941 }
942
943 if (j.contains("checkKalmanMatricesRanks"))
944 {
945 j.at("checkKalmanMatricesRanks").get_to(_checkKalmanMatricesRanks);
946 }
947 if (j.contains("phiCalculationAlgorithm"))
948 {
949 j.at("phiCalculationAlgorithm").get_to(_phiCalculationAlgorithm);
950 }
951 if (j.contains("phiCalculationTaylorOrder"))
952 {
953 j.at("phiCalculationTaylorOrder").get_to(_phiCalculationTaylorOrder);
954 }
955 if (j.contains("qCalculationAlgorithm"))
956 {
957 j.at("qCalculationAlgorithm").get_to(_qCalculationAlgorithm);
958 }
959 // ------------------------------- 𝐐 System/Process noise covariance matrix ---------------------------------
960 if (j.contains("randomProcessAccel"))
961 {
962 j.at("randomProcessAccel").get_to(_randomProcessAccel);
963 }
964 if (j.contains("randomProcessGyro"))
965 {
966 j.at("randomProcessGyro").get_to(_randomProcessGyro);
967 }
968 if (j.contains("stdev_ra"))
969 {
970 _stdev_ra = j.at("stdev_ra");
971 }
972 if (j.contains("stdevAccelNoiseUnits"))
973 {
974 j.at("stdevAccelNoiseUnits").get_to(_stdevAccelNoiseUnits);
975 }
976 if (j.contains("stdev_rg"))
977 {
978 _stdev_rg = j.at("stdev_rg");
979 }
980 if (j.contains("stdevGyroNoiseUnits"))
981 {
982 j.at("stdevGyroNoiseUnits").get_to(_stdevGyroNoiseUnits);
983 }
984 if (j.contains("stdev_bad"))
985 {
986 _stdev_bad = j.at("stdev_bad");
987 }
988 if (j.contains("tau_bad"))
989 {
990 _tau_bad = j.at("tau_bad");
991 }
992 if (j.contains("stdevAccelBiasUnits"))
993 {
994 j.at("stdevAccelBiasUnits").get_to(_stdevAccelBiasUnits);
995 }
996 if (j.contains("stdev_bgd"))
997 {
998 _stdev_bgd = j.at("stdev_bgd");
999 }
1000 if (j.contains("tau_bgd"))
1001 {
1002 _tau_bgd = j.at("tau_bgd");
1003 }
1004 if (j.contains("stdevGyroBiasUnits"))
1005 {
1006 j.at("stdevGyroBiasUnits").get_to(_stdevGyroBiasUnits);
1007 }
1008 if (j.contains("stdevClockFreqUnits"))
1009 {
1010 j.at("stdevClockFreqUnits").get_to(_stdevClockFreqUnits);
1011 }
1012 if (j.contains("stdev_cp"))
1013 {
1014 _stdev_cp = j.at("stdev_cp");
1015 }
1016 if (j.contains("stdev_cf"))
1017 {
1018 _stdev_cf = j.at("stdev_cf");
1019 }
1020 if (j.contains("initBiasAccel"))
1021 {
1022 _initBiasAccel = j.at("initBiasAccel");
1023 }
1024 if (j.contains("initBiasAccelUnit"))
1025 {
1026 _initBiasAccelUnit = j.at("initBiasAccelUnit");
1027 }
1028 if (j.contains("initBiasGyro"))
1029 {
1030 _initBiasGyro = j.at("initBiasGyro");
1031 }
1032 if (j.contains("initBiasGyroUnit"))
1033 {
1034 _initBiasGyroUnit = j.at("initBiasGyroUnit");
1035 }
1036
1037 // TODO: Replace with GNSS Measurement Error Model (see SPP node)
1038 // // -------------------------------- 𝐑 Measurement noise covariance matrix -----------------------------------
1039 // if (j.contains("gnssMeasurementUncertaintyPseudorangeUnit"))
1040 // {
1041 // _gnssMeasurementUncertaintyPseudorangeUnit = j.at("gnssMeasurementUncertaintyPseudorangeUnit");
1042 // }
1043 // if (j.contains("gnssMeasurementUncertaintyPseudorange"))
1044 // {
1045 // _gnssMeasurementUncertaintyPseudorange = j.at("gnssMeasurementUncertaintyPseudorange");
1046 // }
1047 // if (j.contains("gnssMeasurementUncertaintyPseudorangeRateUnit"))
1048 // {
1049 // _gnssMeasurementUncertaintyPseudorangeRateUnit = j.at("gnssMeasurementUncertaintyPseudorangeRateUnit");
1050 // }
1051 // if (j.contains("gnssMeasurementUncertaintyPseudorangeRate"))
1052 // {
1053 // _gnssMeasurementUncertaintyPseudorangeRate = j.at("gnssMeasurementUncertaintyPseudorangeRate");
1054 // }
1055
1056 // -------------------------------------- 𝐏 Error covariance matrix -----------------------------------------
1057 if (j.contains("initCovariancePositionUnit"))
1058 {
1059 j.at("initCovariancePositionUnit").get_to(_initCovariancePositionUnit);
1060 }
1061 if (j.contains("initCovariancePosition"))
1062 {
1063 _initCovariancePosition = j.at("initCovariancePosition");
1064 }
1065 if (j.contains("initCovarianceVelocityUnit"))
1066 {
1067 j.at("initCovarianceVelocityUnit").get_to(_initCovarianceVelocityUnit);
1068 }
1069 if (j.contains("initCovarianceVelocity"))
1070 {
1071 _initCovarianceVelocity = j.at("initCovarianceVelocity");
1072 }
1073 if (j.contains("initCovarianceAttitudeAnglesUnit"))
1074 {
1075 j.at("initCovarianceAttitudeAnglesUnit").get_to(_initCovarianceAttitudeAnglesUnit);
1076 }
1077 if (j.contains("initCovarianceAttitudeAngles"))
1078 {
1079 _initCovarianceAttitudeAngles = j.at("initCovarianceAttitudeAngles");
1080 }
1081 if (j.contains("initCovarianceBiasAccelUnit"))
1082 {
1083 j.at("initCovarianceBiasAccelUnit").get_to(_initCovarianceBiasAccelUnit);
1084 }
1085 if (j.contains("initCovarianceBiasAccel"))
1086 {
1087 _initCovarianceBiasAccel = j.at("initCovarianceBiasAccel");
1088 }
1089 if (j.contains("initCovarianceBiasGyroUnit"))
1090 {
1091 j.at("initCovarianceBiasGyroUnit").get_to(_initCovarianceBiasGyroUnit);
1092 }
1093 if (j.contains("initCovarianceBiasGyro"))
1094 {
1095 _initCovarianceBiasGyro = j.at("initCovarianceBiasGyro");
1096 }
1097 if (j.contains("initCovariancePhaseUnit"))
1098 {
1099 j.at("initCovariancePhaseUnit").get_to(_initCovariancePhaseUnit);
1100 }
1101 if (j.contains("initCovariancePhase"))
1102 {
1103 _initCovariancePhase = j.at("initCovariancePhase");
1104 }
1105 if (j.contains("initCovarianceFreqUnit"))
1106 {
1107 j.at("initCovarianceFreqUnit").get_to(_initCovarianceFreqUnit);
1108 }
1109 if (j.contains("initCovarianceFreq"))
1110 {
1111 _initCovarianceFreq = j.at("initCovarianceFreq");
1112 }
1113}
1114
1115bool NAV::TightlyCoupledKF::initialize()
1116{
1117 LOG_TRACE("{}: called", nameId());
1118
1119 if (std::all_of(inputPins.begin() + INPUT_PORT_INDEX_GNSS_NAV_INFO, inputPins.end(), [](const InputPin& inputPin) { return !inputPin.isPinLinked(); }))
1120 {
1121 LOG_ERROR("{}: You need to connect a GNSS NavigationInfo provider", nameId());
1122 return false;
1123 }
1124
1125 _inertialIntegrator.reset();
1126 _lastImuObs = nullptr;
1127 _externalInitTime.reset();
1128
1129 _recvClk = ReceiverClock({ GPS });
1130
1131 _kalmanFilter.setZero();
1132
1133 // Initial Covariance of the attitude angles in [rad²]
1134 Eigen::Vector3d variance_angles = Eigen::Vector3d::Zero();
1135 if (_initCovarianceAttitudeAnglesUnit == InitCovarianceAttitudeAnglesUnit::rad2)
1136 {
1137 variance_angles = _initCovarianceAttitudeAngles;
1138 }
1139 else if (_initCovarianceAttitudeAnglesUnit == InitCovarianceAttitudeAnglesUnit::deg2)
1140 {
1141 variance_angles = deg2rad(_initCovarianceAttitudeAngles);
1142 }
1143 else if (_initCovarianceAttitudeAnglesUnit == InitCovarianceAttitudeAnglesUnit::rad)
1144 {
1145 variance_angles = _initCovarianceAttitudeAngles.array().pow(2);
1146 }
1147 else if (_initCovarianceAttitudeAnglesUnit == InitCovarianceAttitudeAnglesUnit::deg)
1148 {
1149 variance_angles = deg2rad(_initCovarianceAttitudeAngles).array().pow(2);
1150 }
1151
1152 // Initial Covariance of the velocity in [m²/s²]
1153 Eigen::Vector3d variance_vel = Eigen::Vector3d::Zero();
1154 if (_initCovarianceVelocityUnit == InitCovarianceVelocityUnit::m2_s2)
1155 {
1156 variance_vel = _initCovarianceVelocity;
1157 }
1158 else if (_initCovarianceVelocityUnit == InitCovarianceVelocityUnit::m_s)
1159 {
1160 variance_vel = _initCovarianceVelocity.array().pow(2);
1161 }
1162
1163 // Initial Covariance of the position in [m²]
1164 Eigen::Vector3d e_variance = Eigen::Vector3d::Zero();
1165 // Initial Covariance of the position in [rad² rad² m²]
1166 Eigen::Vector3d lla_variance = Eigen::Vector3d::Zero();
1167 if (_initCovariancePositionUnit == InitCovariancePositionUnit::rad2_rad2_m2)
1168 {
1169 e_variance = trafo::lla2ecef_WGS84(_initCovariancePosition.cwiseSqrt()).array().pow(2);
1170 lla_variance = _initCovariancePosition;
1171 }
1172 else if (_initCovariancePositionUnit == InitCovariancePositionUnit::rad_rad_m)
1173 {
1174 e_variance = trafo::lla2ecef_WGS84(_initCovariancePosition).array().pow(2);
1175 lla_variance = _initCovariancePosition.array().pow(2);
1176 }
1177 else if (_initCovariancePositionUnit == InitCovariancePositionUnit::meter)
1178 {
1179 e_variance = _initCovariancePosition.array().pow(2);
1180 lla_variance = (trafo::ecef2lla_WGS84(trafo::ned2ecef(_initCovariancePosition, Eigen::Vector3d{ 0, 0, 0 }))).array().pow(2);
1181 }
1182 else if (_initCovariancePositionUnit == InitCovariancePositionUnit::meter2)
1183 {
1184 e_variance = _initCovariancePosition;
1185 lla_variance = (trafo::ecef2lla_WGS84(trafo::ned2ecef(_initCovariancePosition.cwiseSqrt(), Eigen::Vector3d{ 0, 0, 0 }))).array().pow(2);
1186 }
1187
1188 // Initial Covariance of the accelerometer biases in [m^2/s^4]
1189 Eigen::Vector3d variance_accelBias = Eigen::Vector3d::Zero();
1190 if (_initCovarianceBiasAccelUnit == InitCovarianceBiasAccelUnit::m2_s4)
1191 {
1192 variance_accelBias = _initCovarianceBiasAccel;
1193 }
1194 else if (_initCovarianceBiasAccelUnit == InitCovarianceBiasAccelUnit::m_s2)
1195 {
1196 variance_accelBias = _initCovarianceBiasAccel.array().pow(2);
1197 }
1198
1199 // Initial Covariance of the gyroscope biases in [rad^2/s^2]
1200 Eigen::Vector3d variance_gyroBias = Eigen::Vector3d::Zero();
1201 if (_initCovarianceBiasGyroUnit == InitCovarianceBiasGyroUnit::rad2_s2)
1202 {
1203 variance_gyroBias = _initCovarianceBiasGyro;
1204 }
1205 else if (_initCovarianceBiasGyroUnit == InitCovarianceBiasGyroUnit::deg2_s2)
1206 {
1207 variance_gyroBias = deg2rad(_initCovarianceBiasGyro.array().sqrt()).array().pow(2);
1208 }
1209 else if (_initCovarianceBiasGyroUnit == InitCovarianceBiasGyroUnit::rad_s)
1210 {
1211 variance_gyroBias = _initCovarianceBiasGyro.array().pow(2);
1212 }
1213 else if (_initCovarianceBiasGyroUnit == InitCovarianceBiasGyroUnit::deg_s)
1214 {
1215 variance_gyroBias = deg2rad(_initCovarianceBiasGyro).array().pow(2);
1216 }
1217
1218 // Initial Covariance of the receiver clock phase drift
1219 double variance_clkPhase{};
1220 if (_initCovariancePhaseUnit == InitCovarianceClockPhaseUnit::m2)
1221 {
1222 variance_clkPhase = _initCovariancePhase;
1223 }
1224 if (_initCovariancePhaseUnit == InitCovarianceClockPhaseUnit::s2)
1225 {
1226 variance_clkPhase = std::pow(InsConst::C, 2) * _initCovariancePhase;
1227 }
1228 if (_initCovariancePhaseUnit == InitCovarianceClockPhaseUnit::m)
1229 {
1230 variance_clkPhase = std::pow(_initCovariancePhase, 2);
1231 }
1232 if (_initCovariancePhaseUnit == InitCovarianceClockPhaseUnit::s)
1233 {
1234 variance_clkPhase = std::pow(InsConst::C * _initCovariancePhase, 2);
1235 }
1236
1237 // Initial Covariance of the receiver clock frequency drift
1238 double variance_clkFreq{};
1239 if (_initCovarianceFreqUnit == InitCovarianceClockFreqUnit::m2_s2)
1240 {
1241 variance_clkFreq = _initCovarianceFreq;
1242 }
1243 if (_initCovarianceFreqUnit == InitCovarianceClockFreqUnit::m_s)
1244 {
1245 variance_clkFreq = std::pow(_initCovarianceFreq, 2);
1246 }
1247
1248 // 𝐏 Error covariance matrix
1249 _kalmanFilter.P = initialErrorCovarianceMatrix_P0(variance_angles, // Flight Angles covariance
1250 variance_vel, // Velocity covariance
1251 _inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::NED
1252 ? lla_variance
1253 : e_variance, // Position (Lat, Lon, Alt) / ECEF covariance
1254 variance_accelBias, // Accelerometer Bias covariance
1255 variance_gyroBias, // Gyroscope Bias covariance
1256 variance_clkPhase, // Receiver clock phase drift covariance
1257 variance_clkFreq); // Receiver clock frequency drift covariance
1258
1259 LOG_DEBUG("{}: initialized", nameId());
1260 LOG_DATA("{}: P_0 =\n{}", nameId(), _kalmanFilter.P);
1261
1262 return true;
1263}
1264
1265void NAV::TightlyCoupledKF::deinitialize()
1266{
1267 LOG_TRACE("{}: called", nameId());
1268}
1269
1270void NAV::TightlyCoupledKF::invokeCallbackWithPosVelAtt(const PosVelAtt& posVelAtt)
1271{
1272 auto tckfSolution = std::make_shared<InsGnssTCKFSolution>();
1273 tckfSolution->insTime = posVelAtt.insTime;
1274 tckfSolution->setPosVelAtt_e(posVelAtt.e_position(), posVelAtt.e_velocity(), posVelAtt.e_Quat_b());
1275
1276 tckfSolution->frame = _inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::NED
1277 ? InsGnssTCKFSolution::Frame::NED
1278 : InsGnssTCKFSolution::Frame::ECEF;
1279 if (_lastImuObs)
1280 {
1281 tckfSolution->b_biasAccel = _lastImuObs->imuPos.b_quat_p() * _inertialIntegrator.p_getLastAccelerationBias();
1282 tckfSolution->b_biasGyro = _lastImuObs->imuPos.b_quat_p() * _inertialIntegrator.p_getLastAngularRateBias();
1283 }
1284 tckfSolution->recvClkOffset = 0; // TODO
1285 tckfSolution->recvClkDrift = 0;
1286 invokeCallbacks(OUTPUT_PORT_INDEX_SOLUTION, tckfSolution);
1287}
1288
1289void NAV::TightlyCoupledKF::recvImuObservation(InputPin::NodeDataQueue& queue, size_t pinIdx)
1290{
1291 auto nodeData = queue.extract_front();
1292 if (nodeData->insTime.empty())
1293 {
1294 LOG_ERROR("{}: Can't set new imuObs__t0 because the observation has no time tag (insTime)", nameId());
1295 return;
1296 }
1297 std::shared_ptr<NAV::PosVelAtt> inertialNavSol = nullptr;
1298
1299 _lastImuObs = std::static_pointer_cast<const ImuObs>(nodeData);
1300
1301 if (!_preferAccelerationOverDeltaMeasurements
1302 && NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(inputPins.at(INPUT_PORT_INDEX_IMU).link.getConnectedPin()->dataIdentifier, { ImuObsWDelta::type() }))
1303 {
1304 auto obs = std::static_pointer_cast<const ImuObsWDelta>(nodeData);
1305 LOG_DATA("{}: recvImuObsWDelta at time [{}]", nameId(), obs->insTime.toYMDHMS());
1306
1307 Eigen::Vector3d p_acceleration = obs->dtime > 1e-12 ? Eigen::Vector3d(obs->dvel / obs->dtime) : Eigen::Vector3d::Zero();
1308 Eigen::Vector3d p_angularRate = obs->dtime > 1e-12 ? Eigen::Vector3d(obs->dtheta / obs->dtime) : Eigen::Vector3d::Zero();
1309
1310 inertialNavSol = _inertialIntegrator.calcInertialSolution(obs->insTime, p_acceleration, p_angularRate, obs->imuPos);
1311 }
1312 else
1313 {
1314 auto obs = std::static_pointer_cast<const ImuObs>(nodeData);
1315 LOG_DATA("{}: recvImuObs at time [{}]", nameId(), obs->insTime.toYMDHMS());
1316
1317 inertialNavSol = _inertialIntegrator.calcInertialSolution(obs->insTime, obs->p_acceleration, obs->p_angularRate, obs->imuPos);
1318 }
1319 if (inertialNavSol && _inertialIntegrator.getMeasurements().back().dt > 1e-8)
1320 {
1321 tightlyCoupledPrediction(inertialNavSol, _inertialIntegrator.getMeasurements().back().dt, std::static_pointer_cast<const ImuObs>(nodeData)->imuPos);
1322
1323 LOG_DATA("{}: e_position = {}", nameId(), inertialNavSol->e_position().transpose());
1324 LOG_DATA("{}: e_velocity = {}", nameId(), inertialNavSol->e_velocity().transpose());
1325 LOG_DATA("{}: rollPitchYaw = {}", nameId(), rad2deg(inertialNavSol->rollPitchYaw()).transpose());
1326 invokeCallbackWithPosVelAtt(*inertialNavSol);
1327 }
1328}
1329
1330void NAV::TightlyCoupledKF::recvGnssObs(InputPin::NodeDataQueue& queue, size_t pinIdx)
1331{
1332 auto obs = std::static_pointer_cast<const GnssObs>(queue.extract_front());
1333 LOG_DATA("{}: recvGnssObs at time [{}]", nameId(), obs->insTime.toYMDHMS());
1334
1335 tightlyCoupledUpdate(obs);
1336}
1337
1338void NAV::TightlyCoupledKF::recvPosVelAttInit(InputPin::NodeDataQueue& queue, size_t pinIdx)
1339{
1340 auto posVelAtt = std::static_pointer_cast<const PosVelAtt>(queue.extract_front());
1341 inputPins[INPUT_PORT_INDEX_POS_VEL_ATT_INIT].queueBlocked = true;
1342 inputPins[INPUT_PORT_INDEX_POS_VEL_ATT_INIT].queue.clear();
1343
1344 LOG_DATA("{}: recvPosVelAttInit at time [{}]", nameId(), posVelAtt->insTime.toYMDHMS());
1345
1346 inputPins[INPUT_PORT_INDEX_GNSS_OBS].priority = 0; // IMU obs (prediction) should be evaluated before the PosVel obs (update)
1347 _externalInitTime = posVelAtt->insTime;
1348
1349 _inertialIntegrator.setInitialState(*posVelAtt);
1350 LOG_DATA("{}: e_position = {}", nameId(), posVelAtt->e_position().transpose());
1351 LOG_DATA("{}: e_velocity = {}", nameId(), posVelAtt->e_velocity().transpose());
1352 LOG_DATA("{}: rollPitchYaw = {}", nameId(), rad2deg(posVelAtt->rollPitchYaw()).transpose());
1353
1354 invokeCallbackWithPosVelAtt(*posVelAtt);
1355}
1356
1357// ###########################################################################################################
1358// Kalman Filter
1359// ###########################################################################################################
1360
1361void NAV::TightlyCoupledKF::tightlyCoupledPrediction(const std::shared_ptr<const PosVelAtt>& inertialNavSol, double tau_i, const ImuPos& imuPos)
1362{
1363 auto dt = fmt::format("{:0.5f}", tau_i);
1364 dt.erase(std::find_if(dt.rbegin(), dt.rend(), [](char ch) { return ch != '0'; }).base(), dt.end()); // NOLINT(boost-use-ranges,modernize-use-ranges) // ranges::find_last_if is C++23 and not supported yet
1365
1366 [[maybe_unused]] InsTime predictTime = inertialNavSol->insTime + std::chrono::duration<double>(tau_i);
1367 LOG_DATA("{}: Predicting (dt = {}s) from [{} - {}] to [{} - {}]", nameId(), dt,
1368 inertialNavSol->insTime.toYMDHMS(), inertialNavSol->insTime.toGPSweekTow(), predictTime.toYMDHMS(), predictTime.toGPSweekTow());
1369
1370 // ------------------------------------------- GUI Parameters ----------------------------------------------
1371
1372 // 𝜎²_ra Variance of the noise on the accelerometer specific-force state [m^2 / s^5]
1373 Eigen::Vector3d sigma2_ra = Eigen::Vector3d::Zero();
1374 switch (_stdevAccelNoiseUnits)
1375 {
1376 case StdevAccelNoiseUnits::mg_sqrtHz: // [mg / √(Hz)]
1377 sigma2_ra = (_stdev_ra * 1e-3 * InsConst::G_NORM).array().square();
1378 break;
1379 case StdevAccelNoiseUnits::m_s2_sqrtHz: // [m / (s^2 · √(Hz))] = [m / (s · √(s))]
1380 sigma2_ra = _stdev_ra.array().square();
1381 break;
1382 }
1383 LOG_DATA("{}: sigma2_ra = {} [m^2 / s^5]", nameId(), sigma2_ra.transpose());
1384
1385 // 𝜎²_rg Variance of the noise on the gyro angular-rate state [rad^2 / s^3]
1386 Eigen::Vector3d sigma2_rg = Eigen::Vector3d::Zero();
1387 switch (_stdevGyroNoiseUnits)
1388 {
1389 case StdevGyroNoiseUnits::deg_hr_sqrtHz: // [deg / hr / √(Hz)] (see Woodman (2007) Chp. 3.2.2 - eq. 7 with seconds instead of hours)
1390 sigma2_rg = (deg2rad(_stdev_rg) / 3600.).array().square();
1391 break;
1392 case StdevGyroNoiseUnits::rad_s_sqrtHz: // [rad / (s · √(Hz))] = [rad / √(s)]
1393 sigma2_rg = _stdev_rg.array().square();
1394 break;
1395 }
1396 LOG_DATA("{}: sigma2_rg = {} [rad^2 / s^3]", nameId(), sigma2_rg.transpose());
1397
1398 // 𝜎²_bad Variance of the accelerometer dynamic bias [m^2 / s^4]
1399 Eigen::Vector3d sigma2_bad = Eigen::Vector3d::Zero();
1400 switch (_stdevAccelBiasUnits)
1401 {
1402 case StdevAccelBiasUnits::microg: // [µg]
1403 sigma2_bad = (_stdev_bad * 1e-6 * InsConst::G_NORM).array().square();
1404 break;
1405 case StdevAccelBiasUnits::m_s2: // [m / s^2]
1406 sigma2_bad = _stdev_bad.array().square();
1407 break;
1408 }
1409 LOG_DATA("{}: sigma2_bad = {} [m^2 / s^4]", nameId(), sigma2_bad.transpose());
1410
1411 // 𝜎²_bgd Variance of the gyro dynamic bias [rad^2 / s^2]
1412 Eigen::Vector3d sigma2_bgd = Eigen::Vector3d::Zero();
1413 switch (_stdevGyroBiasUnits)
1414 {
1415 case StdevGyroBiasUnits::deg_h: // [° / h]
1416 sigma2_bgd = (deg2rad(_stdev_bgd / 3600.0)).array().square();
1417 break;
1418 case StdevGyroBiasUnits::rad_s: // [rad / s]
1419 sigma2_bgd = _stdev_bgd.array().square();
1420 break;
1421 }
1422 LOG_DATA("{}: sigma2_bgd = {} [rad^2 / s^2]", nameId(), sigma2_bgd.transpose());
1423
1424 // 𝜎²_cPhi variance of the receiver clock phase-drift in [m^2]
1425 double sigma2_cPhi{};
1426 switch (_stdevClockPhaseUnits)
1427 {
1428 case StdevClockPhaseUnits::m_sqrtHz: // [m / s^2 / √(Hz)]
1429 sigma2_cPhi = std::pow(_stdev_cp, 2);
1430 break;
1431 }
1432
1433 // 𝜎²_cf variance of the receiver clock frequency drift state [m^2 / s^4 / Hz]
1434 double sigma2_cf{};
1435 switch (_stdevClockFreqUnits)
1436 {
1437 case StdevClockFreqUnits::m_s_sqrtHz: // [m / s^2 / √(Hz)]
1438 sigma2_cf = std::pow(_stdev_cf, 2);
1439 break;
1440 }
1441 LOG_DATA("{}: sigma2_cPhi = {} [m^2]", nameId(), sigma2_cPhi);
1442 LOG_DATA("{}: sigma2_cf = {} [m^2/s^2]", nameId(), sigma2_cf);
1443
1444 // ---------------------------------------------- Prediction -------------------------------------------------
1445
1446 // Latitude 𝜙, longitude λ and altitude (height above ground) in [rad, rad, m] at the time tₖ₋₁
1447 const Eigen::Vector3d& lla_position = inertialNavSol->lla_position();
1448 LOG_DATA("{}: lla_position = {} [rad, rad, m]", nameId(), lla_position.transpose());
1449 // Prime vertical radius of curvature (East/West) [m]
1450 double R_E = calcEarthRadius_E(lla_position(0));
1451 LOG_DATA("{}: R_E = {} [m]", nameId(), R_E);
1452 // Geocentric Radius in [m]
1453 double r_eS_e = calcGeocentricRadius(lla_position(0), R_E);
1454 LOG_DATA("{}: r_eS_e = {} [m]", nameId(), r_eS_e);
1455
1456 auto p_acceleration = _inertialIntegrator.p_calcCurrentAcceleration();
1457 // Acceleration in [m/s^2], in body coordinates
1458 Eigen::Vector3d b_acceleration = p_acceleration
1459 ? imuPos.b_quat_p() * p_acceleration.value()
1460 : Eigen::Vector3d::Zero();
1461 LOG_DATA("{}: b_acceleration = {} [m/s^2]", nameId(), b_acceleration.transpose());
1462
1463 // System Matrix
1464 Eigen::Matrix<double, 17, 17> F;
1465
1466 if (_inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::NED)
1467 {
1468 // n_velocity (tₖ₋₁) Velocity in [m/s], in navigation coordinates, at the time tₖ₋₁
1469 const Eigen::Vector3d& n_velocity = inertialNavSol->n_velocity();
1470 LOG_DATA("{}: n_velocity = {} [m / s]", nameId(), n_velocity.transpose());
1471 // q (tₖ₋₁) Quaternion, from body to navigation coordinates, at the time tₖ₋₁
1472 const Eigen::Quaterniond& n_Quat_b = inertialNavSol->n_Quat_b();
1473 LOG_DATA("{}: n_Quat_b --> Roll, Pitch, Yaw = {} [deg]", nameId(), deg2rad(trafo::quat2eulerZYX(n_Quat_b).transpose()));
1474
1475 // Meridian radius of curvature in [m]
1476 double R_N = calcEarthRadius_N(lla_position(0));
1477 LOG_DATA("{}: R_N = {} [m]", nameId(), R_N);
1478
1479 // Conversion matrix between cartesian and curvilinear perturbations to the position
1480 Eigen::Matrix3d T_rn_p = conversionMatrixCartesianCurvilinear(lla_position, R_N, R_E);
1481 LOG_DATA("{}: T_rn_p =\n{}", nameId(), T_rn_p);
1482
1483 // Gravitation at surface level in [m/s^2]
1484 double g_0 = n_calcGravitation_EGM96(lla_position).norm();
1485
1486 // omega_in^n = omega_ie^n + omega_en^n
1487 Eigen::Vector3d n_omega_in = inertialNavSol->n_Quat_e() * InsConst::e_omega_ie
1488 + n_calcTransportRate(lla_position, n_velocity, R_N, R_E);
1489 LOG_DATA("{}: n_omega_in = {} [rad/s]", nameId(), n_omega_in.transpose());
1490
1491 // System Matrix
1492 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);
1493 LOG_DATA("{}: F =\n{}", nameId(), F);
1494
1495 if (_qCalculationAlgorithm == QCalculationAlgorithm::Taylor1)
1496 {
1497 // 2. Calculate the system noise covariance matrix Q_{k-1}
1498 if (_showKalmanFilterOutputPins)
1499 {
1500 auto guard = requestOutputValueLock(OUTPUT_PORT_INDEX_Q);
1501 _kalmanFilter.Q = n_systemNoiseCovarianceMatrix_Q(sigma2_ra, sigma2_rg,
1502 sigma2_bad, sigma2_bgd,
1503 _tau_bad, _tau_bgd,
1504 sigma2_cPhi, sigma2_cf,
1505 F.block<3, 3>(3, 0), T_rn_p,
1506 n_Quat_b.toRotationMatrix(), tau_i);
1507 notifyOutputValueChanged(OUTPUT_PORT_INDEX_Q, predictTime, guard);
1508 }
1509 else
1510 {
1511 _kalmanFilter.Q = n_systemNoiseCovarianceMatrix_Q(sigma2_ra, sigma2_rg,
1512 sigma2_bad, sigma2_bgd,
1513 _tau_bad, _tau_bgd,
1514 sigma2_cPhi, sigma2_cf,
1515 F.block<3, 3>(3, 0), T_rn_p,
1516 n_Quat_b.toRotationMatrix(), tau_i);
1517 }
1518 }
1519 }
1520 else // if (_inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::ECEF)
1521 {
1522 // e_position (tₖ₋₁) Position in [m/s], in ECEF coordinates, at the time tₖ₋₁
1523 const Eigen::Vector3d& e_position = inertialNavSol->e_position();
1524 LOG_DATA("{}: e_position = {} [m]", nameId(), e_position.transpose());
1525 // q (tₖ₋₁) Quaternion, from body to Earth coordinates, at the time tₖ₋₁
1526 const Eigen::Quaterniond& e_Quat_b = inertialNavSol->e_Quat_b();
1527 LOG_DATA("{}: e_Quat_b = {}", nameId(), e_Quat_b);
1528
1529 // Gravitation in [m/s^2] in ECEF coordinates
1530 Eigen::Vector3d e_gravitation = trafo::e_Quat_n(lla_position(0), lla_position(1)) * n_calcGravitation_EGM96(lla_position);
1531
1532 // System Matrix
1533 F = e_systemMatrix_F(e_Quat_b, b_acceleration, e_position, e_gravitation, r_eS_e, InsConst::e_omega_ie, _tau_bad, _tau_bgd);
1534 LOG_DATA("{}: F =\n{}", nameId(), F);
1535
1536 if (_qCalculationAlgorithm == QCalculationAlgorithm::Taylor1)
1537 {
1538 // 2. Calculate the system noise covariance matrix Q_{k-1}
1539 if (_showKalmanFilterOutputPins)
1540 {
1541 auto guard = requestOutputValueLock(OUTPUT_PORT_INDEX_Q);
1542 _kalmanFilter.Q = e_systemNoiseCovarianceMatrix_Q(sigma2_ra, sigma2_rg,
1543 sigma2_bad, sigma2_bgd,
1544 _tau_bad, _tau_bgd,
1545 sigma2_cPhi, sigma2_cf,
1546 F.block<3, 3>(3, 0),
1547 e_Quat_b.toRotationMatrix(), tau_i);
1548 notifyOutputValueChanged(OUTPUT_PORT_INDEX_Q, predictTime, guard);
1549 }
1550 else
1551 {
1552 _kalmanFilter.Q = e_systemNoiseCovarianceMatrix_Q(sigma2_ra, sigma2_rg,
1553 sigma2_bad, sigma2_bgd,
1554 _tau_bad, _tau_bgd,
1555 sigma2_cPhi, sigma2_cf,
1556 F.block<3, 3>(3, 0),
1557 e_Quat_b.toRotationMatrix(), tau_i);
1558 }
1559 }
1560 }
1561
1562 if (_qCalculationAlgorithm == QCalculationAlgorithm::VanLoan)
1563 {
1564 // Noise Input Matrix
1565 Eigen::Matrix<double, 17, 14> G = noiseInputMatrix_G(_inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::NED
1566 ? inertialNavSol->n_Quat_b()
1567 : inertialNavSol->e_Quat_b());
1568 LOG_DATA("{}: G =\n{}", nameId(), G);
1569
1570 Eigen::Matrix<double, 14, 14> W = noiseScaleMatrix_W(sigma2_ra, sigma2_rg,
1571 sigma2_bad, sigma2_bgd,
1572 _tau_bad, _tau_bgd,
1573 sigma2_cPhi, sigma2_cf);
1574 LOG_DATA("{}: W =\n{}", nameId(), W);
1575
1576 LOG_DATA("{}: G*W*G^T =\n{}", nameId(), G * W * G.transpose());
1577
1578 auto [Phi, Q] = calcPhiAndQWithVanLoanMethod(F, G, W, tau_i);
1579
1580 // 1. Calculate the transition matrix 𝚽_{k-1}
1581 if (_showKalmanFilterOutputPins)
1582 {
1583 auto guard = requestOutputValueLock(OUTPUT_PORT_INDEX_Phi);
1584 _kalmanFilter.Phi = Phi;
1585 notifyOutputValueChanged(OUTPUT_PORT_INDEX_Phi, predictTime, guard);
1586 }
1587 else
1588 {
1589 _kalmanFilter.Phi = Phi;
1590 }
1591
1592 // 2. Calculate the system noise covariance matrix Q_{k-1}
1593 if (_showKalmanFilterOutputPins)
1594 {
1595 auto guard = requestOutputValueLock(OUTPUT_PORT_INDEX_Q);
1596 _kalmanFilter.Q = Q;
1597 notifyOutputValueChanged(OUTPUT_PORT_INDEX_Q, predictTime, guard);
1598 }
1599 else
1600 {
1601 _kalmanFilter.Q = Q;
1602 }
1603 }
1604
1605 // If Q was calculated over Van Loan, then the Phi matrix was automatically calculated with the exponential matrix
1606 if (_phiCalculationAlgorithm != PhiCalculationAlgorithm::Exponential || _qCalculationAlgorithm != QCalculationAlgorithm::VanLoan)
1607 {
1608 auto calcPhi = [&]() {
1609 if (_phiCalculationAlgorithm == PhiCalculationAlgorithm::Exponential)
1610 {
1611 // 1. Calculate the transition matrix 𝚽_{k-1}
1612 _kalmanFilter.Phi = transitionMatrix_Phi_exp(F, tau_i);
1613 }
1614 else if (_phiCalculationAlgorithm == PhiCalculationAlgorithm::Taylor)
1615 {
1616 // 1. Calculate the transition matrix 𝚽_{k-1}
1617 _kalmanFilter.Phi = transitionMatrix_Phi_Taylor(F, tau_i, static_cast<size_t>(_phiCalculationTaylorOrder));
1618 }
1619 else
1620 {
1621 LOG_CRITICAL("{}: Calculation algorithm '{}' for the system matrix Phi is not supported.", nameId(), fmt::underlying(_phiCalculationAlgorithm));
1622 }
1623 };
1624 if (_showKalmanFilterOutputPins)
1625 {
1626 auto guard = requestOutputValueLock(OUTPUT_PORT_INDEX_Phi);
1627 calcPhi();
1628 notifyOutputValueChanged(OUTPUT_PORT_INDEX_Phi, predictTime, guard);
1629 }
1630 else
1631 {
1632 calcPhi();
1633 }
1634 }
1635
1636 LOG_DATA("{}: KF.Phi =\n{}", nameId(), _kalmanFilter.Phi);
1637 LOG_DATA("{}: KF.Q =\n{}", nameId(), _kalmanFilter.Q);
1638
1639 LOG_DATA("{}: Q - Q^T =\n{}", nameId(), _kalmanFilter.Q - _kalmanFilter.Q.transpose());
1640 LOG_DATA("{}: KF.P (before prediction) =\n{}", nameId(), _kalmanFilter.P);
1641
1642 // 3. Propagate the state vector estimate from x(+) and x(-)
1643 // 4. Propagate the error covariance matrix from P(+) and P(-)
1644 if (_showKalmanFilterOutputPins)
1645 {
1646 auto guard1 = requestOutputValueLock(OUTPUT_PORT_INDEX_x);
1647 auto guard2 = requestOutputValueLock(OUTPUT_PORT_INDEX_P);
1648 _kalmanFilter.predict();
1649 notifyOutputValueChanged(OUTPUT_PORT_INDEX_x, predictTime, guard1);
1650 notifyOutputValueChanged(OUTPUT_PORT_INDEX_P, predictTime, guard2);
1651 }
1652 else
1653 {
1654 _kalmanFilter.predict();
1655 }
1656
1657 LOG_DATA("{}: KF.x = {}", nameId(), _kalmanFilter.x.transpose());
1658 LOG_DATA("{}: KF.P (after prediction) =\n{}", nameId(), _kalmanFilter.P);
1659
1660 // Averaging of P to avoid numerical problems with symmetry (did not work)
1661 // _kalmanFilter.P = ((_kalmanFilter.P + _kalmanFilter.P.transpose()) / 2.0);
1662
1663 // LOG_DEBUG("{}: F\n{}\n", nameId(), F);
1664 // LOG_DEBUG("{}: Phi\n{}\n", nameId(), _kalmanFilter.Phi);
1665
1666 // LOG_DEBUG("{}: Q\n{}\n", nameId(), _kalmanFilter.Q);
1667 // LOG_DEBUG("{}: Q - Q^T\n{}\n", nameId(), _kalmanFilter.Q - _kalmanFilter.Q.transpose());
1668
1669 // LOG_DEBUG("{}: x\n{}\n", nameId(), _kalmanFilter.x);
1670
1671 // LOG_DEBUG("{}: P\n{}\n", nameId(), _kalmanFilter.P);
1672 // LOG_DEBUG("{}: P - P^T\n{}\n", nameId(), _kalmanFilter.P - _kalmanFilter.P.transpose());
1673
1674 if (_checkKalmanMatricesRanks)
1675 {
1676 Eigen::FullPivLU<Eigen::MatrixXd> lu(_kalmanFilter.P);
1677 auto rank = lu.rank();
1678 if (rank != _kalmanFilter.P.rows())
1679 {
1680 LOG_WARN("{}: P.rank = {}", nameId(), rank);
1681 }
1682 }
1683}
1684
1685void NAV::TightlyCoupledKF::tightlyCoupledUpdate(const std::shared_ptr<const GnssObs>& gnssObs)
1686{
1687 // TODO: Rework node
1688 // LOG_DATA("{}: Updating to [{}]", nameId(), gnssObs->insTime);
1689
1690 // // Collection of all connected navigation data providers
1691 // std::vector<const GnssNavInfo*> gnssNavInfos;
1692 // for (size_t i = 0; i < _nNavInfoPins; i++)
1693 // {
1694 // if (const auto* gnssNavInfo = getInputValue<const GnssNavInfo>(INPUT_PORT_INDEX_GNSS_NAV_INFO + i))
1695 // {
1696 // gnssNavInfos.push_back(gnssNavInfo);
1697 // }
1698 // }
1699 // if (gnssNavInfos.empty()) { return; }
1700
1701 // // TODO: Replace with GNSS Measurement Error Model (see SPP node)
1702 // // GnssMeasurementErrorModel gnssMeasurementErrorModel;
1703 // // switch (_gnssMeasurementUncertaintyPseudorangeUnit)
1704 // // {
1705 // // case GnssMeasurementUncertaintyPseudorangeUnit::meter2:
1706 // // gnssMeasurementErrorModel.rtklibParams.carrierPhaseErrorAB[0] = std::sqrt(_gnssMeasurementUncertaintyPseudorange);
1707 // // gnssMeasurementErrorModel.rtklibParams.carrierPhaseErrorAB[1] = std::sqrt(_gnssMeasurementUncertaintyPseudorange);
1708 // // break;
1709 // // case GnssMeasurementUncertaintyPseudorangeUnit::meter:
1710 // // gnssMeasurementErrorModel.rtklibParams.carrierPhaseErrorAB[0] = _gnssMeasurementUncertaintyPseudorange;
1711 // // gnssMeasurementErrorModel.rtklibParams.carrierPhaseErrorAB[1] = _gnssMeasurementUncertaintyPseudorange;
1712 // // break;
1713 // // }
1714 // // switch (_gnssMeasurementUncertaintyPseudorangeRateUnit)
1715 // // {
1716 // // case GnssMeasurementUncertaintyPseudorangeRateUnit::m2_s2:
1717 // // gnssMeasurementErrorModel.rtklibParams.dopplerFrequency = rangeRate2doppler(std::sqrt(_gnssMeasurementUncertaintyPseudorangeRate), G01);
1718 // // break;
1719 // // case GnssMeasurementUncertaintyPseudorangeRateUnit::m_s:
1720 // // gnssMeasurementErrorModel.rtklibParams.dopplerFrequency = rangeRate2doppler(_gnssMeasurementUncertaintyPseudorangeRate, G01);
1721 // // break;
1722 // // }
1723
1724 // if (!_inertialIntegrator.hasInitialPosition()) // Calculate a SPP solution and use it to initialize
1725 // {
1726 // if (auto sppSol = SPP::calcSppSolutionLSE(SPP::State{}, gnssObs, gnssNavInfos,
1727 // _ionosphereModel, _troposphereModels, gnssMeasurementErrorModel,
1728 // SPP::EstimatorType::WEIGHTED_LEAST_SQUARES,
1729 // _filterFreq, _filterCode, _excludedSatellites, _elevationMask,
1730 // true, _interSysErrs, _interSysDrifts))
1731 // {
1732 // PosVelAtt posVelAtt;
1733 // posVelAtt.insTime = sppSol->insTime;
1734 // posVelAtt.setPosVelAtt_n(sppSol->lla_position(), sppSol->n_velocity(),
1735 // trafo::n_Quat_b(deg2rad(_initalRollPitchYaw[0]), deg2rad(_initalRollPitchYaw[1]), deg2rad(_initalRollPitchYaw[2])));
1736
1737 // _inertialIntegrator.setInitialState(posVelAtt);
1738
1739 // LOG_DATA("{}: e_position = {}", nameId(), posVelAtt.e_position().transpose());
1740 // LOG_DATA("{}: lla_position = {}, {}, {}", nameId(), rad2deg(posVelAtt.lla_position().x()), rad2deg(posVelAtt.lla_position().y()), posVelAtt.lla_position().z());
1741 // LOG_DATA("{}: e_velocity = {}", nameId(), posVelAtt.e_velocity().transpose());
1742 // LOG_DATA("{}: rollPitchYaw = {}", nameId(), rad2deg(posVelAtt.rollPitchYaw()).transpose());
1743 // }
1744 // else
1745 // {
1746 // return;
1747 // }
1748 // }
1749
1750 // const auto& latestInertialNavSol = _inertialIntegrator.getLatestState().value().get();
1751
1752 // // -------------------------------------------- GUI Parameters -----------------------------------------------
1753
1754 // // GNSS measurement uncertainty for the pseudorange (Variance σ²) in [m^2]
1755 // double gnssSigmaSquaredPseudorange{};
1756 // switch (_gnssMeasurementUncertaintyPseudorangeUnit)
1757 // {
1758 // case GnssMeasurementUncertaintyPseudorangeUnit::meter:
1759 // gnssSigmaSquaredPseudorange = std::pow(_gnssMeasurementUncertaintyPseudorange, 2);
1760 // break;
1761 // case GnssMeasurementUncertaintyPseudorangeUnit::meter2:
1762 // gnssSigmaSquaredPseudorange = _gnssMeasurementUncertaintyPseudorange;
1763 // break;
1764 // }
1765 // LOG_DATA("{}: gnssSigmaSquaredPseudorange = {} [m^2]", nameId(), gnssSigmaSquaredPseudorange);
1766
1767 // // GNSS measurement uncertainty for the pseudorange-rate (Variance σ²) in [m^2/s^2]
1768 // double gnssSigmaSquaredPseudorangeRate{};
1769 // switch (_gnssMeasurementUncertaintyPseudorangeRateUnit)
1770 // {
1771 // case GnssMeasurementUncertaintyPseudorangeRateUnit::m_s:
1772 // gnssSigmaSquaredPseudorangeRate = std::pow(_gnssMeasurementUncertaintyPseudorangeRate, 2);
1773 // break;
1774 // case GnssMeasurementUncertaintyPseudorangeRateUnit::m2_s2:
1775 // gnssSigmaSquaredPseudorangeRate = _gnssMeasurementUncertaintyPseudorangeRate;
1776 // break;
1777 // }
1778 // LOG_DATA("{}: gnssSigmaSquaredPseudorangeRate = {} [m^2/s^2]", nameId(), gnssSigmaSquaredPseudorangeRate);
1779
1780 // // TODO: Check whether it is necessary to distinguish the following three types (see Groves eq. 9.168)
1781 // double sigma_rhoZ = std::sqrt(gnssSigmaSquaredPseudorange);
1782 // // double sigma_rhoC{};
1783 // // double sigma_rhoA{};
1784 // double sigma_rZ = std::sqrt(gnssSigmaSquaredPseudorangeRate);
1785 // // double sigma_rC{};
1786 // // double sigma_rA{};
1787
1788 // // ----------------------------------------- Read observation data -------------------------------------------
1789
1790 // // Collection of all connected Ionospheric Corrections
1791 // IonosphericCorrections ionosphericCorrections;
1792 // for (const auto* gnssNavInfo : gnssNavInfos)
1793 // {
1794 // for (const auto& correction : gnssNavInfo->ionosphericCorrections.data())
1795 // {
1796 // if (!ionosphericCorrections.contains(correction.satSys, correction.alphaBeta))
1797 // {
1798 // ionosphericCorrections.insert(correction.satSys, correction.alphaBeta, correction.data);
1799 // }
1800 // }
1801 // }
1802
1803 // // Data calculated for each satellite (only satellites filtered by GUI filter & NAV data available)
1804 // std::vector<SPP::CalcData> calcData = SPP::selectObservations(gnssObs, gnssNavInfos, _filterFreq, _filterCode, _excludedSatellites);
1805 // // Sorted list of satellite systems
1806 // std::set<SatelliteSystem> availSatelliteSystems;
1807 // for (const auto& calc : calcData) { availSatelliteSystems.insert(calc.obsData.satSigId.toSatId().satSys); }
1808
1809 // size_t nParam = 4 + availSatelliteSystems.size() - 1; // 3x pos, 1x clk, (N-1)x clkDiff
1810 // LOG_DATA("{}: nParam {}", nameId(), nParam);
1811
1812 // size_t nMeasPsr = calcData.size();
1813 // LOG_DATA("{}: nMeasPsr {}", nameId(), nMeasPsr);
1814
1815 // // Find all observations providing a doppler measurement (for velocity calculation)
1816 // size_t nDopplerMeas = SPP::findDopplerMeasurements(calcData);
1817 // LOG_DATA("{}: nDopplerMeas {}", nameId(), nDopplerMeas);
1818
1819 // std::vector<SatelliteSystem> satelliteSystems;
1820 // satelliteSystems.reserve(availSatelliteSystems.size());
1821 // std::copy(availSatelliteSystems.begin(), availSatelliteSystems.end(), std::back_inserter(satelliteSystems));
1822
1823 // const Eigen::Vector3d& lla_position = latestInertialNavSol.lla_position();
1824 // const Eigen::Vector3d& e_position = latestInertialNavSol.e_position();
1825 // const Eigen::Vector3d& e_velocity = latestInertialNavSol.e_velocity();
1826
1827 // auto state = SPP::State{ .e_position = e_position,
1828 // .e_velocity = e_velocity,
1829 // .recvClk = _recvClk };
1830 // // _recvClk.bias.value += _kalmanFilter.x(15, 0) / InsConst::C;
1831 // auto sppSol = std::make_shared<SppSolution>(); // TODO: Make the next function not require a sppSol by splitting it into a second function
1832 // SPP::calcDataBasedOnEstimates(sppSol, satelliteSystems, calcData, state,
1833 // nParam, nMeasPsr, nDopplerMeas, gnssObs->insTime, lla_position,
1834 // _elevationMask, SPP::EstimatorType::KF);
1835
1836 // if (sppSol->nMeasPsr + sppSol->nMeasDopp == 0)
1837 // {
1838 // return; // Do not update, as we do not have any observations
1839 // }
1840
1841 // SPP::getInterSysKeys(satelliteSystems, _interSysErrs, _interSysDrifts);
1842
1843 // auto [e_H_psr, // Measurement/Geometry matrix for the pseudorange
1844 // psrEst, // Pseudorange estimates [m]
1845 // psrMeas, // Pseudorange measurements [m]
1846 // W_psr, // Pseudorange measurement error weight matrix
1847 // dpsr, // Difference between Pseudorange measurements and estimates
1848 // e_H_r, // Measurement/Geometry matrix for the pseudorange-rate
1849 // psrRateEst, // Corrected pseudorange-rate estimates [m/s]
1850 // psrRateMeas, // Corrected pseudorange-rate measurements [m/s]
1851 // W_psrRate, // Pseudorange rate (doppler) measurement error weight matrix
1852 // dpsr_dot // Difference between Pseudorange rate measurements and estimates
1853 // ] = calcMeasurementEstimatesAndDesignMatrix(sppSol, calcData,
1854 // gnssObs->insTime,
1855 // state, lla_position,
1856 // ionosphericCorrections, _ionosphereModel,
1857 // _troposphereModels, gnssMeasurementErrorModel,
1858 // SPP::EstimatorType::KF, true, _interSysErrs, _interSysDrifts);
1859
1860 // // double tau_epoch = !_lastEpochTime.empty()
1861 // // ? static_cast<double>((gnssObs->insTime - _lastEpochTime).count())
1862 // // : 0.0;
1863 // // LOG_DATA("{}: tau_epoch = {}", nameId(), tau_epoch);
1864 // // _lastEpochTime = gnssObs->insTime;
1865 // // for (auto& calc : calcData)
1866 // // {
1867 // // if (calc.skipped) { continue; }
1868 // // // Pseudorange estimate [m]
1869 // // psrEst(static_cast<int>(ix)) = geometricDist
1870 // // + _recvClk.bias.value * InsConst::C
1871 // // + _recvClk.drift.value * InsConst::C * tau_epoch // TODO: Should we also do this in SPP KF and here?
1872 // // - calc.satClkBias * InsConst::C
1873 // // + dpsr_I
1874 // // + dpsr_T
1875 // // + dpsr_ie;
1876 // // }
1877
1878 // // ---------------------------------------------- Update -----------------------------------------------------
1879
1880 // if (_inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::NED)
1881 // {
1882 // // Prime vertical radius of curvature (East/West) [m]
1883 // double R_E = calcEarthRadius_E(lla_position(0));
1884 // LOG_DATA("{}: R_E = {} [m]", nameId(), R_E);
1885 // // Meridian radius of curvature in [m]
1886 // double R_N = calcEarthRadius_N(lla_position(0));
1887 // LOG_DATA("{}: R_N = {} [m]", nameId(), R_N);
1888
1889 // std::vector<Eigen::Vector3d> n_lineOfSightUnitVectors;
1890 // n_lineOfSightUnitVectors.resize(sppSol->nMeasPsr);
1891 // std::vector<double> satElevation;
1892 // satElevation.resize(sppSol->nMeasPsr);
1893 // // std::vector<double> CN0; // TODO: get this from GnssObs
1894 // // std::vector<double> rangeAccel; // TODO: get this from GnssObs
1895
1896 // std::vector<double> pseudoRangeObservations;
1897 // pseudoRangeObservations.resize(sppSol->nMeasPsr);
1898 // std::vector<double> pseudoRangeRateObservations;
1899 // pseudoRangeRateObservations.resize(sppSol->nMeasPsr);
1900
1901 // size_t ix = 0;
1902
1903 // for (auto& calc : calcData)
1904 // {
1905 // LOG_DATA("calc.n_lineOfSightUnitVector.transpose() = {}, calc.skipped = {}", calc.n_lineOfSightUnitVector.transpose(), calc.skipped);
1906 // if (calc.skipped) { continue; }
1907
1908 // n_lineOfSightUnitVectors[ix] = calc.n_lineOfSightUnitVector;
1909 // LOG_DATA("n_lineOfSightUnitVectors[{}] = {}", ix, n_lineOfSightUnitVectors[ix].transpose());
1910 // satElevation[ix] = calc.satElevation;
1911 // LOG_DATA("satElevation[{}] = {}", ix, satElevation[ix]);
1912 // // CN0[i] = calc // TODO: get CN0 from data
1913 // // rangeAccel[i] = calc // TODO: get rangeAccel from data
1914 // pseudoRangeObservations[ix] = psrMeas(SPP::Meas::Psr{ calc.obsData.satSigId });
1915 // pseudoRangeRateObservations[ix] = calc.pseudorangeRateMeas.value();
1916
1917 // ix++;
1918 // }
1919
1920 // // 5. Calculate the measurement matrix H_k
1921 // if (_showKalmanFilterOutputPins) { requestOutputValueLock(OUTPUT_PORT_INDEX_H); }
1922
1923 // _kalmanFilter.H = n_measurementMatrix_H(R_N, R_E, lla_position, n_lineOfSightUnitVectors, pseudoRangeRateObservations);
1924 // LOG_DATA("{}: kalmanFilter.H =\n{}", nameId(), _kalmanFilter.H);
1925
1926 // // 6. Calculate the measurement noise covariance matrix R_k
1927 // if (_showKalmanFilterOutputPins) { requestOutputValueLock(OUTPUT_PORT_INDEX_R); }
1928
1929 // _kalmanFilter.R = measurementNoiseCovariance_R(sigma_rhoZ, sigma_rZ, satElevation);
1930 // LOG_DATA("{}: kalmanFilter.R =\n{}", nameId(), _kalmanFilter.R);
1931
1932 // std::vector<double> pseudoRangeEstimates;
1933 // pseudoRangeEstimates.resize(ix);
1934 // std::vector<double> pseudoRangeRateEstimates;
1935 // pseudoRangeRateEstimates.resize(ix);
1936 // for (size_t obsIdx = 0; obsIdx < n_lineOfSightUnitVectors.size(); obsIdx++)
1937 // {
1938 // pseudoRangeEstimates[obsIdx] = psrEst(all)(static_cast<Eigen::Index>(obsIdx));
1939 // if (psrRateEst.rows() != 0)
1940 // {
1941 // pseudoRangeRateEstimates[obsIdx] = psrRateEst(all)(static_cast<Eigen::Index>(obsIdx));
1942 // }
1943 // }
1944
1945 // // 8. Formulate the measurement z_k
1946 // if (_showKalmanFilterOutputPins) { requestOutputValueLock(OUTPUT_PORT_INDEX_z); }
1947
1948 // _kalmanFilter.z = measurementInnovation_dz(pseudoRangeObservations, pseudoRangeEstimates, pseudoRangeRateObservations, pseudoRangeRateEstimates);
1949 // LOG_DATA("{}: _kalmanFilter.z =\n{}", nameId(), _kalmanFilter.z);
1950 // }
1951 // else // if (_inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::ECEF)
1952 // {
1953 // LOG_ERROR("{}: Update in ECEF-frame not implemented, yet.", nameId()); // TODO: implement update in e-Sys.
1954 // }
1955
1956 // if (_showKalmanFilterOutputPins)
1957 // {
1958 // notifyOutputValueChanged(OUTPUT_PORT_INDEX_H, gnssObs->insTime);
1959 // notifyOutputValueChanged(OUTPUT_PORT_INDEX_R, gnssObs->insTime);
1960 // notifyOutputValueChanged(OUTPUT_PORT_INDEX_z, gnssObs->insTime);
1961 // }
1962 // LOG_DATA("{}: KF.H =\n{}", nameId(), _kalmanFilter.H);
1963 // LOG_DATA("{}: KF.R =\n{}", nameId(), _kalmanFilter.R);
1964 // LOG_DATA("{}: KF.z =\n{}", nameId(), _kalmanFilter.z);
1965
1966 // if (_checkKalmanMatricesRanks && _kalmanFilter.H.rows() > 0) // Number of rows of H is 0, if there is no pseudorange in one epoch. Better skip this than crashing.
1967 // {
1968 // Eigen::FullPivLU<Eigen::MatrixXd> lu(_kalmanFilter.H * _kalmanFilter.P * _kalmanFilter.H.transpose() + _kalmanFilter.R);
1969 // auto rank = lu.rank();
1970 // if (rank != _kalmanFilter.H.rows())
1971 // {
1972 // LOG_WARN("{}: (HPH^T + R).rank = {}", nameId(), rank);
1973 // }
1974 // }
1975
1976 // // 7. Calculate the Kalman gain matrix K_k
1977 // // 9. Update the state vector estimate from x(-) to x(+)
1978 // // 10. Update the error covariance matrix from P(-) to P(+)
1979 // if (_showKalmanFilterOutputPins)
1980 // {
1981 // requestOutputValueLock(OUTPUT_PORT_INDEX_K);
1982 // requestOutputValueLock(OUTPUT_PORT_INDEX_x);
1983 // requestOutputValueLock(OUTPUT_PORT_INDEX_P);
1984 // }
1985
1986 // _kalmanFilter.correctWithMeasurementInnovation();
1987
1988 // if (_showKalmanFilterOutputPins)
1989 // {
1990 // notifyOutputValueChanged(OUTPUT_PORT_INDEX_K, gnssObs->insTime);
1991 // notifyOutputValueChanged(OUTPUT_PORT_INDEX_x, gnssObs->insTime);
1992 // notifyOutputValueChanged(OUTPUT_PORT_INDEX_P, gnssObs->insTime);
1993 // }
1994 // LOG_DATA("{}: KF.K =\n{}", nameId(), _kalmanFilter.K);
1995 // LOG_DATA("{}: KF.x =\n{}", nameId(), _kalmanFilter.x);
1996 // LOG_DATA("{}: KF.P =\n{}", nameId(), _kalmanFilter.P);
1997
1998 // if (_checkKalmanMatricesRanks && _kalmanFilter.H.rows() > 0) // Number of rows of H is 0, if there is no pseudorange in one epoch. Better skip this than crashing.
1999 // {
2000 // Eigen::FullPivLU<Eigen::MatrixXd> lu(_kalmanFilter.H * _kalmanFilter.P * _kalmanFilter.H.transpose() + _kalmanFilter.R);
2001 // auto rank = lu.rank();
2002 // if (rank != _kalmanFilter.H.rows())
2003 // {
2004 // LOG_WARN("{}: (HPH^T + R).rank = {}", nameId(), rank);
2005 // }
2006
2007 // Eigen::FullPivLU<Eigen::MatrixXd> luP(_kalmanFilter.P);
2008 // rank = luP.rank();
2009 // if (rank != _kalmanFilter.P.rows())
2010 // {
2011 // LOG_WARN("{}: P.rank = {}", nameId(), rank);
2012 // }
2013 // }
2014
2015 // _recvClk.bias.value += _kalmanFilter.x(15, 0) / InsConst::C;
2016 // _recvClk.drift.value += _kalmanFilter.x(16, 0) / InsConst::C;
2017
2018 // // Push out the new data
2019 // auto tckfSolution = std::make_shared<InsGnssTCKFSolution>();
2020 // tckfSolution->insTime = gnssObs->insTime;
2021 // tckfSolution->positionError = _kalmanFilter.x.block<3, 1>(6, 0);
2022 // tckfSolution->velocityError = _kalmanFilter.x.block<3, 1>(3, 0);
2023 // tckfSolution->attitudeError = _kalmanFilter.x.block<3, 1>(0, 0) * (1. / SCALE_FACTOR_ATTITUDE);
2024
2025 // if (_lastImuObs)
2026 // {
2027 // _inertialIntegrator.applySensorBiasesIncrements(_lastImuObs->imuPos.p_quatAccel_b() * -_kalmanFilter.x.block<3, 1>(9, 0) * (1. / SCALE_FACTOR_ACCELERATION),
2028 // _lastImuObs->imuPos.p_quatGyro_b() * -_kalmanFilter.x.block<3, 1>(12, 0) * (1. / SCALE_FACTOR_ANGULAR_RATE));
2029 // }
2030 // tckfSolution->b_biasAccel = _inertialIntegrator.p_getLastAccelerationBias();
2031 // tckfSolution->b_biasGyro = _inertialIntegrator.p_getLastAngularRateBias();
2032 // tckfSolution->recvClkOffset = _recvClk.bias.value * InsConst::C;
2033 // tckfSolution->recvClkDrift = _recvClk.drift.value * InsConst::C;
2034
2035 // if (_inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::NED)
2036 // {
2037 // tckfSolution->positionError = tckfSolution->positionError.array() * Eigen::Array3d(1. / SCALE_FACTOR_LAT_LON, 1. / SCALE_FACTOR_LAT_LON, 1);
2038 // tckfSolution->frame = InsGnssTCKFSolution::Frame::NED;
2039 // _inertialIntegrator.applyStateErrors_n(tckfSolution->positionError, tckfSolution->velocityError, tckfSolution->attitudeError);
2040 // const auto& state = _inertialIntegrator.getLatestState().value().get();
2041 // tckfSolution->setPosVelAtt_n(state.lla_position(), state.n_velocity(), state.n_Quat_b());
2042 // }
2043 // LOG_DATA("tckfSolution->positionError = {}", tckfSolution->positionError.transpose());
2044 // LOG_DATA("tckfSolution->velocityError = {}", tckfSolution->velocityError.transpose());
2045 // LOG_DATA("tckfSolution->attitudeError = {}", tckfSolution->attitudeError.transpose());
2046 // LOG_DATA("tckfSolution->b_biasAccel = {}", tckfSolution->b_biasAccel.transpose());
2047 // LOG_DATA("tckfSolution->b_biasGyro = {}", tckfSolution->b_biasGyro.transpose());
2048 // LOG_DATA("tckfSolution->recvClkOffset = {}", tckfSolution->recvClkOffset);
2049 // LOG_DATA("tckfSolution->recvClkDrift = {}", tckfSolution->recvClkDrift);
2050
2051 // // Closed loop
2052 // if (_showKalmanFilterOutputPins) { requestOutputValueLock(OUTPUT_PORT_INDEX_x); }
2053
2054 // _kalmanFilter.x.setZero();
2055
2056 // invokeCallbacks(OUTPUT_PORT_INDEX_SOLUTION, tckfSolution);
2057}
2058
2059// ###########################################################################################################
2060// System matrix 𝐅
2061// ###########################################################################################################
2062
2063Eigen::Matrix<double, 17, 17> NAV::TightlyCoupledKF::n_systemMatrix_F(const Eigen::Quaterniond& n_Quat_b,
2064 const Eigen::Vector3d& b_specForce_ib,
2065 const Eigen::Vector3d& n_omega_in,
2066 const Eigen::Vector3d& n_velocity,
2067 const Eigen::Vector3d& lla_position,
2068 double R_N,
2069 double R_E,
2070 double g_0,
2071 double r_eS_e,
2072 const Eigen::Vector3d& tau_bad,
2073 const Eigen::Vector3d& tau_bgd) const
2074{
2075 double latitude = lla_position(0); // Geodetic latitude of the body in [rad]
2076 double altitude = lla_position(2); // Geodetic height of the body in [m]
2077
2078 Eigen::Vector3d beta_bad = 1. / tau_bad.array(); // Gauss-Markov constant for the accelerometer 𝛽 = 1 / 𝜏 (𝜏 correlation length)
2079 Eigen::Vector3d beta_bgd = 1. / tau_bgd.array(); // Gauss-Markov constant for the gyroscope 𝛽 = 1 / 𝜏 (𝜏 correlation length)
2080
2081 // System matrix 𝐅
2082 // 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{0}_{3,1} & \mathbf{0}_{3,1} \\ \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,1} & \mathbf{0}_{3,1} \\ \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,1} & \mathbf{0}_{3,1} \\ \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \vee -\mathbf{\beta} & \mathbf{0}_3 & \mathbf{0}_{3,1} & \mathbf{0}_{3,1} \\ \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \vee -\mathbf{\beta} & \mathbf{0}_{3,1} & \mathbf{0}_{3,1} \\ \mathbf{0}_{1,3} & \mathbf{0}_{1,3} & \mathbf{0}_{1,3} & \mathbf{0}_{1,3} & \mathbf{0}_{1,3} & 0 & 1 \\ \mathbf{0}_{1,3} & \mathbf{0}_{1,3} & \mathbf{0}_{1,3} & \mathbf{0}_{1,3} & \mathbf{0}_{1,3} & 0 & 0 \end{pmatrix}
2083 Eigen::MatrixXd F = Eigen::MatrixXd::Zero(17, 17);
2084
2085 F.block<3, 3>(0, 0) = n_F_dpsi_dpsi(n_omega_in);
2086 F.block<3, 3>(0, 3) = n_F_dpsi_dv(latitude, altitude, R_N, R_E);
2087 F.block<3, 3>(0, 6) = n_F_dpsi_dr(latitude, altitude, n_velocity, R_N, R_E);
2088 F.block<3, 3>(0, 12) = n_F_dpsi_dw(n_Quat_b.toRotationMatrix());
2089 F.block<3, 3>(3, 0) = n_F_dv_dpsi(n_Quat_b * b_specForce_ib);
2090 F.block<3, 3>(3, 3) = n_F_dv_dv(n_velocity, latitude, altitude, R_N, R_E);
2091 F.block<3, 3>(3, 6) = n_F_dv_dr(n_velocity, latitude, altitude, R_N, R_E, g_0, r_eS_e);
2092 F.block<3, 3>(3, 9) = n_F_dv_df(n_Quat_b.toRotationMatrix());
2093 F.block<3, 3>(6, 3) = n_F_dr_dv(latitude, altitude, R_N, R_E);
2094 F.block<3, 3>(6, 6) = n_F_dr_dr(n_velocity, latitude, altitude, R_N, R_E);
2095 if (_qCalculationAlgorithm == QCalculationAlgorithm::VanLoan)
2096 {
2097 F.block<3, 3>(9, 9) = n_F_df_df(_randomProcessAccel == RandomProcess::RandomWalk ? Eigen::Vector3d::Zero() : beta_bad);
2098 F.block<3, 3>(12, 12) = n_F_dw_dw(_randomProcessGyro == RandomProcess::RandomWalk ? Eigen::Vector3d::Zero() : beta_bgd);
2099 }
2100
2101 F.middleRows<3>(0) *= SCALE_FACTOR_ATTITUDE; // 𝜓' [deg / s] = 180/π * ... [rad / s]
2102 F.middleCols<3>(0) *= 1. / SCALE_FACTOR_ATTITUDE;
2103
2104 // F.middleRows<3>(3) *= 1.; // 𝛿v' [m / s^2] = 1 * [m / s^2]
2105 // F.middleCols<3>(3) *= 1. / 1.;
2106
2107 F.middleRows<2>(6) *= SCALE_FACTOR_LAT_LON; // 𝛿ϕ' [pseudometre / s] = R0 * [rad / s]
2108 F.middleCols<2>(6) *= 1. / SCALE_FACTOR_LAT_LON;
2109 // F.middleRows<1>(8) *= 1.; // 𝛿h' [m / s] = 1 * [m / s]
2110 // F.middleCols<1>(8) *= 1. / 1.;
2111
2112 F.middleRows<3>(9) *= SCALE_FACTOR_ACCELERATION; // 𝛿f' [mg / s] = 1e3 / g * [m / s^3]
2113 F.middleCols<3>(9) *= 1. / SCALE_FACTOR_ACCELERATION;
2114
2115 F.middleRows<3>(12) *= SCALE_FACTOR_ANGULAR_RATE; // 𝛿ω' [mrad / s^2] = 1e3 * [rad / s^2]
2116 F.middleCols<3>(12) *= 1. / SCALE_FACTOR_ANGULAR_RATE;
2117
2118 // Change in clock offset = clock drift
2119 F(15, 16) = 1;
2120
2121 return F;
2122}
2123
2124Eigen::Matrix<double, 17, 17> NAV::TightlyCoupledKF::e_systemMatrix_F(const Eigen::Quaterniond& e_Quat_b,
2125 const Eigen::Vector3d& b_specForce_ib,
2126 const Eigen::Vector3d& e_position,
2127 const Eigen::Vector3d& e_gravitation,
2128 double r_eS_e,
2129 const Eigen::Vector3d& e_omega_ie,
2130 const Eigen::Vector3d& tau_bad,
2131 const Eigen::Vector3d& tau_bgd) const
2132{
2133 Eigen::Vector3d beta_bad = 1. / tau_bad.array(); // Gauss-Markov constant for the accelerometer 𝛽 = 1 / 𝜏 (𝜏 correlation length)
2134 Eigen::Vector3d beta_bgd = 1. / tau_bgd.array(); // Gauss-Markov constant for the gyroscope 𝛽 = 1 / 𝜏 (𝜏 correlation length)
2135
2136 // System matrix 𝐅
2137 // Math: \mathbf{F}^e = \begin{pmatrix} \mathbf{F}_{\dot{\psi},\psi}^e & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{C}_b^e & \mathbf{0}_{3,1} & \mathbf{0}_{3,1} \\ \mathbf{F}_{\delta \dot{v},\psi}^e & \mathbf{F}_{\delta \dot{v},\delta v}^e & \mathbf{F}_{\delta \dot{v},\delta r}^e & \mathbf{C}_b^e & \mathbf{0}_3 & \mathbf{0}_{3,1} & \mathbf{0}_{3,1} \\ \mathbf{0}_3 & \mathbf{F}_{\delta \dot{r},\delta v}^e & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_{3,1} & \mathbf{0}_{3,1} \\ \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \vee -\mathbf{\beta} & \mathbf{0}_3 & \mathbf{0}_{3,1} & \mathbf{0}_{3,1} \\ \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \vee -\mathbf{\beta} & \mathbf{0}_{3,1} & \mathbf{0}_{3,1} \\ \mathbf{0}_{1,3} & \mathbf{0}_{1,3} & \mathbf{0}_{1,3} & \mathbf{0}_{1,3} & \mathbf{0}_{1,3} & 0 & 1 \\ \mathbf{0}_{1,3} & \mathbf{0}_{1,3} & \mathbf{0}_{1,3} & \mathbf{0}_{1,3} & \mathbf{0}_{1,3} & 0 & 0 \end{pmatrix}
2138 Eigen::MatrixXd F = Eigen::MatrixXd::Zero(17, 17);
2139
2140 F.block<3, 3>(0, 0) = e_F_dpsi_dpsi(e_omega_ie.z());
2141 F.block<3, 3>(0, 12) = e_F_dpsi_dw(e_Quat_b.toRotationMatrix());
2142 F.block<3, 3>(3, 0) = e_F_dv_dpsi(e_Quat_b * b_specForce_ib);
2143 F.block<3, 3>(3, 3) = e_F_dv_dv(e_omega_ie.z());
2144 F.block<3, 3>(3, 6) = e_F_dv_dr(e_position, e_gravitation, r_eS_e, e_omega_ie);
2145 F.block<3, 3>(3, 9) = e_F_dv_df_b(e_Quat_b.toRotationMatrix());
2146 F.block<3, 3>(6, 3) = e_F_dr_dv();
2147 if (_qCalculationAlgorithm == QCalculationAlgorithm::VanLoan)
2148 {
2149 F.block<3, 3>(9, 9) = e_F_df_df(_randomProcessAccel == RandomProcess::RandomWalk ? Eigen::Vector3d::Zero() : beta_bad);
2150 F.block<3, 3>(12, 12) = e_F_dw_dw(_randomProcessGyro == RandomProcess::RandomWalk ? Eigen::Vector3d::Zero() : beta_bgd);
2151 }
2152
2153 F.middleRows<3>(0) *= SCALE_FACTOR_ATTITUDE; // 𝜓' [deg / s] = 180/π * ... [rad / s]
2154 F.middleCols<3>(0) *= 1. / SCALE_FACTOR_ATTITUDE;
2155
2156 // F.middleRows<3>(3) *= 1.; // 𝛿v' [m / s^2] = 1 * [m / s^2]
2157 // F.middleCols<3>(3) *= 1. / 1.;
2158
2159 // F.middleRows<3>(6) *= 1.; // 𝛿r' [m / s] = 1 * [m / s]
2160 // F.middleCols<3>(6) *= 1. / 1.;
2161
2162 F.middleRows<3>(9) *= SCALE_FACTOR_ACCELERATION; // 𝛿f' [mg / s] = 1e3 / g * [m / s^3]
2163 F.middleCols<3>(9) *= 1. / SCALE_FACTOR_ACCELERATION;
2164
2165 F.middleRows<3>(12) *= SCALE_FACTOR_ANGULAR_RATE; // 𝛿ω' [mrad / s^2] = 1e3 * [rad / s^2]
2166 F.middleCols<3>(12) *= 1. / SCALE_FACTOR_ANGULAR_RATE;
2167
2168 // Change in clock offset = clock drift
2169 F(15, 16) = 1;
2170
2171 return F;
2172}
2173
2174// ###########################################################################################################
2175// Noise input matrix 𝐆 & Noise scale matrix 𝐖
2176// System noise covariance matrix 𝐐
2177// ###########################################################################################################
2178
2179Eigen::Matrix<double, 17, 14> NAV::TightlyCoupledKF::noiseInputMatrix_G(const Eigen::Quaterniond& ien_Quat_b)
2180{
2181 // DCM matrix from body to navigation frame
2182 Eigen::Matrix3d ien_Dcm_b = ien_Quat_b.toRotationMatrix();
2183
2184 // Math: \mathbf{G}_{a} = \begin{bmatrix} \mathbf{G}_{INS} & 0 \\ 0 & \mathbf{G}_{GNSS} \end{bmatrix} = \begin{bmatrix} -\mathbf{C}_b^{i,e,n} & 0 & 0 & 0 & 0 & 0 \\ 0 & \mathbf{C}_b^{i,e,n} & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & \mathbf{I}_3 & 0 & 0 & 0 \\ 0 & 0 & 0 & \mathbf{I}_3 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix}
2185 Eigen::Matrix<double, 17, 14> G = Eigen::Matrix<double, 17, 14>::Zero();
2186
2187 // G_INS
2188 G.block<3, 3>(0, 0) = SCALE_FACTOR_ATTITUDE * -ien_Dcm_b;
2189 G.block<3, 3>(3, 3) = ien_Dcm_b;
2190 G.block<3, 3>(9, 6) = SCALE_FACTOR_ACCELERATION * Eigen::Matrix3d::Identity();
2191 G.block<3, 3>(12, 9) = SCALE_FACTOR_ANGULAR_RATE * Eigen::Matrix3d::Identity();
2192
2193 // G_GNSS
2194 G.block<2, 2>(15, 12) = Eigen::Matrix2d::Identity();
2195
2196 return G;
2197}
2198
2199Eigen::Matrix<double, 14, 14> NAV::TightlyCoupledKF::noiseScaleMatrix_W(const Eigen::Vector3d& sigma2_ra, const Eigen::Vector3d& sigma2_rg,
2200 const Eigen::Vector3d& sigma2_bad, const Eigen::Vector3d& sigma2_bgd,
2201 const Eigen::Vector3d& tau_bad, const Eigen::Vector3d& tau_bgd,
2202 const double& sigma2_cPhi, const double& sigma2_cf)
2203{
2204 // Math: \mathbf{W} = \begin{bmatrix} \mathbf{W}_{INS} & 0 \\ 0 & \mathbf{W}_{GNSS} \end{bmatrix}
2205 Eigen::Matrix<double, 14, 14> W = Eigen::Matrix<double, 14, 14>::Zero();
2206
2207 // W_INS
2208 W.block<3, 3>(0, 0).diagonal() = sigma2_rg; // S_rg
2209 W.block<3, 3>(3, 3).diagonal() = sigma2_ra; // S_ra
2210 W.block<3, 3>(6, 6).diagonal() = _randomProcessAccel == RandomProcess::RandomWalk ? sigma2_bad : psdBiasGaussMarkov(sigma2_bad, tau_bad); // S_bad
2211 W.block<3, 3>(9, 9).diagonal() = _randomProcessGyro == RandomProcess::RandomWalk ? sigma2_bgd : psdBiasGaussMarkov(sigma2_bgd, tau_bgd); // S_bgd
2212
2213 // W_GNSS
2214 W(12, 12) = sigma2_cPhi; // S_cPhi
2215 W(13, 13) = sigma2_cf; // S_cf
2216
2217 return W;
2218}
2219
2220Eigen::Matrix<double, 17, 17> NAV::TightlyCoupledKF::n_systemNoiseCovarianceMatrix_Q(const Eigen::Vector3d& sigma2_ra, const Eigen::Vector3d& sigma2_rg,
2221 const Eigen::Vector3d& sigma2_bad, const Eigen::Vector3d& sigma2_bgd,
2222 const Eigen::Vector3d& tau_bad, const Eigen::Vector3d& tau_bgd,
2223 const double& sigma2_cPhi, const double& sigma2_cf,
2224 const Eigen::Matrix3d& n_F_21, const Eigen::Matrix3d& T_rn_p,
2225 const Eigen::Matrix3d& n_Dcm_b, const double& tau_s)
2226{
2227 // Math: \mathbf{Q}^n = \begin{pmatrix} \mathbf{Q}_{INS}^n & 0 \\ 0 & \mathbf{Q}_{GNSS} \end{pmatrix} \ \mathrm{with} \ \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} \ \text{P. Groves}\,(14.80) \ \mathrm{and} \ \mathbf{Q}_{GNSS} = \begin{pmatrix} S_{c\phi}\tau_s + \frac{1}{3}S_{cf}\tau_s^3 & \frac{1}{2}S_{cf}\tau_s^2 \\ \frac{1}{2}S_{cf}\tau_s^2 & S_{cf}\tau_s \end{pmatrix} \ \text{P. Groves}\,(14.88)
2228 Eigen::Vector3d S_ra = sigma2_ra * tau_s;
2229 Eigen::Vector3d S_rg = sigma2_rg * tau_s;
2230 Eigen::Vector3d S_bad = sigma2_bad.array() / tau_bad.array();
2231 Eigen::Vector3d S_bgd = sigma2_bgd.array() / tau_bgd.array();
2232
2233 // double S_cPhi = psdClockPhaseDrift(sigma2_cPhi, tau_s);
2234 // double S_cf = psdClockFreqDrift(sigma2_cf, tau_s);
2235
2236 Eigen::Matrix3d b_Dcm_n = n_Dcm_b.transpose();
2237
2238 Eigen::Matrix<double, 17, 17> Q = Eigen::Matrix<double, 17, 17>::Zero();
2239 Q.block<3, 3>(0, 0) = Q_psi_psi(S_rg, S_bgd, tau_s); // Q_11
2240 Q.block<3, 3>(3, 0) = ien_Q_dv_psi(S_rg, S_bgd, n_F_21, tau_s); // Q_21
2241 Q.block<3, 3>(3, 3) = ien_Q_dv_dv(S_ra, S_bad, S_rg, S_bgd, n_F_21, tau_s); // Q_22
2242 Q.block<3, 3>(3, 12) = ien_Q_dv_domega(S_bgd, n_F_21, n_Dcm_b, tau_s); // Q_25
2243 Q.block<3, 3>(6, 0) = n_Q_dr_psi(S_rg, S_bgd, n_F_21, T_rn_p, tau_s); // Q_31
2244 Q.block<3, 3>(6, 3) = n_Q_dr_dv(S_ra, S_bad, S_rg, S_bgd, n_F_21, T_rn_p, tau_s); // Q_32
2245 Q.block<3, 3>(6, 6) = n_Q_dr_dr(S_ra, S_bad, S_rg, S_bgd, n_F_21, T_rn_p, tau_s); // Q_33
2246 Q.block<3, 3>(6, 9) = n_Q_dr_df(S_bgd, T_rn_p, n_Dcm_b, tau_s); // Q_34
2247 Q.block<3, 3>(6, 12) = n_Q_dr_domega(S_bgd, n_F_21, T_rn_p, n_Dcm_b, tau_s); // Q_35
2248 Q.block<3, 3>(9, 3) = Q_df_dv(S_bad, b_Dcm_n, tau_s); // Q_42
2249 Q.block<3, 3>(9, 9) = Q_df_df(S_bad, tau_s); // Q_44
2250 Q.block<3, 3>(12, 0) = Q_domega_psi(S_bgd, b_Dcm_n, tau_s); // Q_51
2251 Q.block<3, 3>(12, 12) = Q_domega_domega(S_bgd, tau_s); // Q_55
2252
2253 Q.block<3, 3>(0, 3) = Q.block<3, 3>(3, 0).transpose(); // Q_21^T
2254 Q.block<3, 3>(0, 6) = Q.block<3, 3>(6, 0).transpose(); // Q_31^T
2255 Q.block<3, 3>(3, 6) = Q.block<3, 3>(6, 3).transpose(); // Q_32^T
2256 Q.block<3, 3>(9, 6) = Q.block<3, 3>(6, 9).transpose(); // Q_34^T
2257 Q.block<3, 3>(12, 3) = Q.block<3, 3>(3, 12).transpose(); // Q_25^T
2258 Q.block<3, 3>(12, 6) = Q.block<3, 3>(6, 12).transpose(); // Q_35^T
2259 Q.block<3, 3>(3, 9) = Q.block<3, 3>(9, 3).transpose(); // Q_42^T
2260 Q.block<3, 3>(0, 12) = Q.block<3, 3>(12, 0).transpose(); // Q_51^T
2261
2262 Q.middleRows<3>(0) *= SCALE_FACTOR_ATTITUDE;
2263 Q.middleRows<2>(6) *= SCALE_FACTOR_LAT_LON;
2264 Q.middleRows<3>(9) *= SCALE_FACTOR_ACCELERATION;
2265 Q.middleRows<3>(12) *= SCALE_FACTOR_ANGULAR_RATE;
2266
2267 Q.middleCols<3>(0) *= SCALE_FACTOR_ATTITUDE;
2268 Q.middleCols<2>(6) *= SCALE_FACTOR_LAT_LON;
2269 Q.middleCols<3>(9) *= SCALE_FACTOR_ACCELERATION;
2270 Q.middleCols<3>(12) *= SCALE_FACTOR_ANGULAR_RATE;
2271
2272 Q.block<2, 2>(15, 15) = Q_gnss(sigma2_cPhi, sigma2_cf, tau_s);
2273
2274 return Q;
2275}
2276
2277Eigen::Matrix<double, 17, 17> NAV::TightlyCoupledKF::e_systemNoiseCovarianceMatrix_Q(const Eigen::Vector3d& sigma2_ra, const Eigen::Vector3d& sigma2_rg,
2278 const Eigen::Vector3d& sigma2_bad, const Eigen::Vector3d& sigma2_bgd,
2279 const Eigen::Vector3d& tau_bad, const Eigen::Vector3d& tau_bgd,
2280 const double& sigma2_cPhi, const double& sigma2_cf,
2281 const Eigen::Matrix3d& e_F_21,
2282 const Eigen::Matrix3d& e_Dcm_b, const double& tau_s)
2283{
2284 // Math: \mathbf{Q}^e = \begin{pmatrix} \mathbf{Q}_{INS}^e & 0 \\ 0 & \mathbf{Q}_{GNSS} \end{pmatrix} \ \mathrm{with} \ \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} \ \text{P. Groves}\,(14.80) \ \mathrm{and} \ \mathbf{Q}_{GNSS} = \begin{pmatrix} S_{c\phi}\tau_s + \frac{1}{3}S_{cf}\tau_s^3 & \frac{1}{2}S_{cf}\tau_s^2 \\ \frac{1}{2}S_{cf}\tau_s^2 & S_{cf}\tau_s \end{pmatrix} \ \text{P. Groves}\,(14.88)
2285
2286 Eigen::Vector3d S_ra = sigma2_ra * tau_s;
2287 Eigen::Vector3d S_rg = sigma2_rg * tau_s;
2288 Eigen::Vector3d S_bad = sigma2_bad.array() / tau_bad.array();
2289 Eigen::Vector3d S_bgd = sigma2_bgd.array() / tau_bgd.array();
2290
2291 // double S_cPhi = psdClockPhaseDrift(sigma2_cPhi, tau_s);
2292 // double S_cf = psdClockFreqDrift(sigma2_cf, tau_s);
2293
2294 Eigen::Matrix3d b_Dcm_e = e_Dcm_b.transpose();
2295
2296 Eigen::Matrix<double, 17, 17> Q = Eigen::Matrix<double, 17, 17>::Zero();
2297 Q.block<3, 3>(0, 0) = Q_psi_psi(S_rg, S_bgd, tau_s); // Q_11
2298 Q.block<3, 3>(3, 0) = ien_Q_dv_psi(S_rg, S_bgd, e_F_21, tau_s); // Q_21
2299 Q.block<3, 3>(3, 3) = ien_Q_dv_dv(S_ra, S_bad, S_rg, S_bgd, e_F_21, tau_s); // Q_22
2300 Q.block<3, 3>(3, 12) = ien_Q_dv_domega(S_bgd, e_F_21, e_Dcm_b, tau_s); // Q_25
2301 Q.block<3, 3>(6, 0) = ie_Q_dr_psi(S_rg, S_bgd, e_F_21, tau_s); // Q_31
2302 Q.block<3, 3>(6, 3) = ie_Q_dr_dv(S_ra, S_bad, S_rg, S_bgd, e_F_21, tau_s); // Q_32
2303 Q.block<3, 3>(6, 6) = ie_Q_dr_dr(S_ra, S_bad, S_rg, S_bgd, e_F_21, tau_s); // Q_33
2304 Q.block<3, 3>(6, 9) = ie_Q_dr_df(S_bgd, e_Dcm_b, tau_s); // Q_34
2305 Q.block<3, 3>(6, 12) = ie_Q_dr_domega(S_bgd, e_F_21, e_Dcm_b, tau_s); // Q_35
2306 Q.block<3, 3>(9, 3) = Q_df_dv(S_bad, b_Dcm_e, tau_s); // Q_42
2307 Q.block<3, 3>(9, 9) = Q_df_df(S_bad, tau_s); // Q_44
2308 Q.block<3, 3>(12, 0) = Q_domega_psi(S_bgd, b_Dcm_e, tau_s); // Q_51
2309 Q.block<3, 3>(12, 12) = Q_domega_domega(S_bgd, tau_s); // Q_55
2310
2311 Q.block<3, 3>(0, 3) = Q.block<3, 3>(3, 0).transpose(); // Q_21^T
2312 Q.block<3, 3>(0, 6) = Q.block<3, 3>(6, 0).transpose(); // Q_31^T
2313 Q.block<3, 3>(3, 6) = Q.block<3, 3>(6, 3).transpose(); // Q_32^T
2314 Q.block<3, 3>(9, 6) = Q.block<3, 3>(6, 9).transpose(); // Q_34^T
2315 Q.block<3, 3>(12, 3) = Q.block<3, 3>(3, 12).transpose(); // Q_25^T
2316 Q.block<3, 3>(12, 6) = Q.block<3, 3>(6, 12).transpose(); // Q_35^T
2317 Q.block<3, 3>(3, 9) = Q.block<3, 3>(9, 3).transpose(); // Q_42^T
2318 Q.block<3, 3>(0, 12) = Q.block<3, 3>(12, 0).transpose(); // Q_51^T
2319
2320 Q.middleRows<3>(0) *= SCALE_FACTOR_ATTITUDE;
2321 Q.middleRows<3>(9) *= SCALE_FACTOR_ACCELERATION;
2322 Q.middleRows<3>(12) *= SCALE_FACTOR_ANGULAR_RATE;
2323
2324 Q.middleCols<3>(0) *= SCALE_FACTOR_ATTITUDE;
2325 Q.middleCols<3>(9) *= SCALE_FACTOR_ACCELERATION;
2326 Q.middleCols<3>(12) *= SCALE_FACTOR_ANGULAR_RATE;
2327
2328 Q.block<2, 2>(15, 15) = Q_gnss(sigma2_cPhi, sigma2_cf, tau_s);
2329
2330 return Q;
2331}
2332
2333// ###########################################################################################################
2334// Error covariance matrix P
2335// ###########################################################################################################
2336
2337Eigen::Matrix<double, 17, 17> NAV::TightlyCoupledKF::initialErrorCovarianceMatrix_P0(const Eigen::Vector3d& variance_angles,
2338 const Eigen::Vector3d& variance_vel,
2339 const Eigen::Vector3d& variance_pos,
2340 const Eigen::Vector3d& variance_accelBias,
2341 const Eigen::Vector3d& variance_gyroBias,
2342 const double& variance_clkPhase,
2343 const double& variance_clkFreq) const
2344{
2345 double scaleFactorPosition = _inertialIntegrator.getIntegrationFrame() == InertialIntegrator::IntegrationFrame::NED
2346 ? SCALE_FACTOR_LAT_LON
2347 : 1.0;
2348
2349 // 𝐏 Error covariance matrix
2350 Eigen::Matrix<double, 17, 17> P = Eigen::Matrix<double, 17, 17>::Zero();
2351
2352 P.diagonal() << std::pow(SCALE_FACTOR_ATTITUDE, 2) * variance_angles, // Flight Angles covariance
2353 variance_vel, // Velocity covariance
2354 std::pow(scaleFactorPosition, 2) * variance_pos(0), // Latitude/Pos X covariance
2355 std::pow(scaleFactorPosition, 2) * variance_pos(1), // Longitude/Pos Y covariance
2356 variance_pos(2), // Altitude/Pos Z covariance
2357 std::pow(SCALE_FACTOR_ACCELERATION, 2) * variance_accelBias, // Accelerometer Bias covariance
2358 std::pow(SCALE_FACTOR_ANGULAR_RATE, 2) * variance_gyroBias, // Gyroscope Bias covariance
2359 variance_clkPhase, // Receiver clock phase drift covariance
2360 variance_clkFreq; // Receiver clock frequency drift covariance
2361
2362 return P;
2363}
2364
2365// ###########################################################################################################
2366// Update
2367// ###########################################################################################################
2368
2369Eigen::MatrixXd NAV::TightlyCoupledKF::n_measurementMatrix_H(const double& R_N, const double& R_E, const Eigen::Vector3d& lla_position, const std::vector<Eigen::Vector3d>& n_lineOfSightUnitVectors, std::vector<double>& pseudoRangeRateObservations)
2370{
2371 // Math: \mathbf{H}_{G,k}^n \approx \begin{pmatrix} 0_{1,3} & 0_{1,3} & {\mathbf{h}_{\rho p}^1}^\text{T} & 0_{1,3} & 0_{1,3} & 1 & 0 \\ 0_{1,3} & 0_{1,3} & {\mathbf{h}_{\rho p}^2}^\text{T} & 0_{1,3} & 0_{1,3} & 1 & 0 \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ 0_{1,3} & 0_{1,3} & {\mathbf{h}_{\rho p}^m}^\text{T} & 0_{1,3} & 0_{1,3} & 1 & 0 \\ - & - & - & - & - & - & - \\ 0_{1,3} & {\mathbf{u}_{a1}^n}^\text{T} & 0_{1,3} & 0_{1,3} & 0_{1,3} & 0 & 1 \\ 0_{1,3} & {\mathbf{u}_{a2}^n}^\text{T} & 0_{1,3} & 0_{1,3} & 0_{1,3} & 0 & 1 \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ 0_{1,3} & {\mathbf{u}_{am}^n}^\text{T} & 0_{1,3} & 0_{1,3} & 0_{1,3} & 0 & 1 \end{pmatrix}_{\mathbf{x} = \hat{\mathbf{x}}_k^-} \qquad \text{P. Groves}\,(14.127)
2372
2373 auto numSats = static_cast<uint8_t>(n_lineOfSightUnitVectors.size());
2374
2375 auto numMeasurements = static_cast<Eigen::Index>(2 * numSats);
2376 Eigen::MatrixXd H = Eigen::MatrixXd::Zero(numMeasurements, 17);
2377
2378 Eigen::Vector3d h_rhoP;
2379
2380 for (size_t j = 0; j < numSats; j++)
2381 {
2382 h_rhoP << (R_N + lla_position(2)) * n_lineOfSightUnitVectors[j](0),
2383 (R_E + lla_position(2)) * std::cos(lla_position(0)) * n_lineOfSightUnitVectors[j](1),
2384 -n_lineOfSightUnitVectors[j](2);
2385
2386 auto i = static_cast<uint8_t>(j);
2387
2388 H.block<1, 3>(i, 6) = h_rhoP.transpose();
2389 H(i, 15) = 1;
2390
2391 // Take pseudorange-rate observation only into account, if available (otherwise, this ruins the stochastics)
2392 if (!std::isnan(pseudoRangeRateObservations[j]))
2393 {
2394 H.block<1, 3>(numSats + i, 3) = n_lineOfSightUnitVectors[j].transpose();
2395 H(numSats + i, 16) = 1;
2396 }
2397 }
2398
2399 // H.middleCols<3>(0) *= 1. / SCALE_FACTOR_ATTITUDE; // Only zero elements
2400 H.middleCols<2>(6) *= 1. / SCALE_FACTOR_LAT_LON;
2401 // H.middleCols<3>(9) *= 1. / SCALE_FACTOR_ACCELERATION; // Only zero elements
2402 // H.middleCols<3>(12) *= 1. / SCALE_FACTOR_ANGULAR_RATE; // Only zero elements
2403
2404 return H;
2405}
2406
2407Eigen::MatrixXd NAV::TightlyCoupledKF::measurementNoiseCovariance_R(const double& sigma_rhoZ, const double& sigma_rZ, const std::vector<double>& satElevation)
2408{
2409 // Math: \mathbf{R}_G = \begin{pmatrix} \sigma_{\rho1}^2 & 0 & \dots & 0 & | & 0 & 0 & \dots & 0 \\ 0 & \sigma_{\rho2}^2 & \dots & 0 & | & 0 & 0 & \dots & 0 \\ \vdots & \vdots & \ddots & \vdots & | & \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \dots & \sigma_{\rho m}^2 & | & 0 & 0 & \dots & 0 \\ - & - & - & - & - & - & - & - & - \\ 0 & 0 & \dots & 0 & | & \sigma_{r1}^2 & 0 & \dots & 0 \\ 0 & 0 & \dots & 0 & | & 0 & \sigma_{r 2}^2 & \dots & 0 \\ \vdots & \vdots & \ddots & \vdots & | & \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \dots & 0 & | & 0 & 0 & \dots & \sigma_{rm}^2 \end{pmatrix} \qquad \text{P. Groves}\,(9.168)
2410
2411 auto numSats = static_cast<uint8_t>(satElevation.size());
2412
2413 auto numMeasurements = static_cast<Eigen::Index>(2 * numSats);
2414 Eigen::MatrixXd R = Eigen::MatrixXd::Zero(numMeasurements, numMeasurements);
2415
2416 for (size_t j = 0; j < numSats; j++)
2417 {
2418 auto sigma_rho2 = sigma2(satElevation[j], sigma_rhoZ, 0., 0., 0., 0.);
2419 auto sigma_r2 = sigma2(satElevation[j], sigma_rZ, 0., 0., 0., 0.);
2420
2421 auto i = static_cast<uint8_t>(j);
2422
2423 R(i, i) = sigma_rho2;
2424 R(numSats + i, numSats + i) = sigma_r2;
2425 }
2426
2427 return R;
2428}
2429
2430double NAV::TightlyCoupledKF::sigma2(const double& satElevation, const double& sigma_Z, const double& sigma_C, const double& sigma_A, const double& CN0, const double& rangeAccel)
2431{
2432 // Math: \sigma_{\rho j}^2 = \frac{1}{\sin^2{\theta_{nu}^{aj}}}\left(\sigma_{\rho Z}^2 + \frac{\sigma_{\rho c}^2}{(c/n_0)_j} + \sigma_{\rho a}^2 \ddot{r}_{aj}^2 \right) \qquad \text{P. Groves}\,(9.168)\,\left(\text{extension of}\,(9.137)\right)
2433
2434 auto sigma2 = 1. / std::pow(std::sin(satElevation), 2) * std::pow(sigma_Z, 2);
2435 if (CN0 != 0.)
2436 {
2437 sigma2 += 1. / std::pow(std::sin(satElevation), 2) * std::pow(sigma_C, 2) / CN0;
2438 }
2439 if (rangeAccel != 0.)
2440 {
2441 sigma2 += 1. / std::pow(std::sin(satElevation), 2) * std::pow(sigma_A, 2) * std::pow(rangeAccel, 2);
2442 }
2443
2444 return sigma2;
2445}
2446
2447Eigen::MatrixXd NAV::TightlyCoupledKF::measurementInnovation_dz(const std::vector<double>& pseudoRangeObservations, const std::vector<double>& pseudoRangeEstimates, const std::vector<double>& pseudoRangeRateObservations, const std::vector<double>& pseudoRangeRateEstimates)
2448{
2449 // Math: \delta \mathbf{z}^-_{\mathbf{G},k} = \begin{pmatrix} \delta \mathbf{z}^-_{\rho,k} \\ \delta \mathbf{z}^-_{r,k} \end{pmatrix} = \begin{pmatrix} \rho^1_{a,C} - \hat{\rho}^{1-}_{a,C}, \rho^2_{a,C} - \hat{\rho}^{2-}_{a,C}, \cdots, \rho^m_{a,C} - \hat{\rho}^{m-}_{a,C} \\ \dot{\rho}^1_{a,C} - \hat{\dot{\rho}}^{1-}_{a,C}, \dot{\rho}^2_{a,C} - \hat{\dot{\rho}}^{2-}_{a,C}, \cdots, \dot{\rho}^m_{a,C} - \hat{\dot{\rho}}^{m-}_{a,C} \end{pmatrix}_k \qquad \text{P. Groves}\,(14.119)
2450
2451 auto numSats = static_cast<uint8_t>(pseudoRangeObservations.size());
2452
2453 auto numMeasurements = static_cast<Eigen::Index>(2 * numSats);
2454 Eigen::MatrixXd deltaZ = Eigen::MatrixXd::Zero(numMeasurements, 1);
2455
2456 for (size_t j = 0; j < numSats; j++)
2457 {
2458 deltaZ(static_cast<Eigen::Index>(j), 0) = pseudoRangeObservations[j] - pseudoRangeEstimates[j];
2459 if (!std::isnan(pseudoRangeRateObservations[j]))
2460 {
2461 deltaZ(static_cast<Eigen::Index>(numSats + j), 0) = pseudoRangeRateObservations[j] - pseudoRangeRateEstimates[j];
2462 }
2463 }
2464
2465 return deltaZ;
2466}
2467
2468*/
TightlyCoupledKF.hpp
Kalman Filter class for the tightly coupled INS/GNSS integration.