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