INSTINCT Code Coverage Report

Directory: src/
File: Navigation/GNSS/Positioning/SPP/KalmanFilter.cpp
Date: 2025-07-19 10:51:51
Exec Total Coverage
Lines: 38 158 24.1%
Functions: 3 13 23.1%
Branches: 22 159 13.8%
Line Branch Exec Source
1 // This file is part of INSTINCT, the INS Toolkit for Integrated
2 // Navigation Concepts and Training by the Institute of Navigation of
3 // the University of Stuttgart, Germany.
4 //
5 // This Source Code Form is subject to the terms of the Mozilla Public
6 // License, v. 2.0. If a copy of the MPL was not distributed with this
7 // file, You can obtain one at https://mozilla.org/MPL/2.0/.
8
9 /// @file KalmanFilter.cpp
10 /// @brief SPP Kalman Filter
11 /// @author T. Topp (topp@ins.uni-stuttgart.de)
12 /// @date 2023-12-22
13
14 #include "KalmanFilter.hpp"
15
16 #include <algorithm>
17 #include <functional>
18 #include <variant>
19
20 #include "Navigation/GNSS/Core/SatelliteSystem.hpp"
21 #include "Navigation/GNSS/Positioning/SPP/Keys.hpp"
22 #include "Navigation/GNSS/SystemModel/InterFrequencyBiasModel.hpp"
23 #include "Navigation/GNSS/SystemModel/ReceiverClockModel.hpp"
24 #include "Navigation/GNSS/SystemModel/SystemModel.hpp"
25 #include "internal/gui/widgets/InputWithUnit.hpp"
26 #include "Navigation/Transformations/CoordinateFrames.hpp"
27 #include "util/Logger.hpp"
28 #include <Eigen/src/Core/Matrix.h>
29 #include <fmt/format.h>
30
31 namespace NAV::SPP
32 {
33 27 void KalmanFilter::reset(const std::vector<SatelliteSystem>& satelliteSystems)
34 {
35 // Covariance of the P matrix initialization velocity uncertainty [m²/s²]
36
1/3
✓ Branch 0 taken 27 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 27 switch (_gui_initCovarianceVelocityUnit)
37 {
38 27 case InitCovarianceVelocityUnits::m_s:
39 27 _initCovarianceVelocity = std::pow(_gui_initCovarianceVelocity, 2);
40 27 break;
41 ✗ case InitCovarianceVelocityUnits::m2_s2:
42 ✗ _initCovarianceVelocity = _gui_initCovarianceVelocity;
43 ✗ break;
44 }
45
46 // Covariance of the P matrix initialization clock drift uncertainty [m²/s²]
47
1/5
✗ Branch 0 not taken.
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 27 switch (_gui_initCovarianceClockDriftUnit)
48 {
49 ✗ case InitCovarianceClockDriftUnits::m_s:
50 ✗ _initCovarianceClockDrift = std::pow(_gui_initCovarianceClockDrift, 2);
51 ✗ break;
52 27 case InitCovarianceClockDriftUnits::s_s:
53 27 _initCovarianceClockDrift = std::pow(_gui_initCovarianceClockDrift * InsConst::C, 2);
54 27 break;
55 ✗ case InitCovarianceClockDriftUnits::m2_s2:
56 ✗ _initCovarianceClockDrift = _gui_initCovarianceClockDrift;
57 ✗ break;
58 ✗ case InitCovarianceClockDriftUnits::s2_s2:
59 ✗ _initCovarianceClockDrift = _gui_initCovarianceClockDrift * std::pow(InsConst::C, 2);
60 ✗ break;
61 }
62
63 // Covariance of the P matrix initialization inter system clock drift uncertainty [m²/s²]
64
1/5
✗ Branch 0 not taken.
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 27 switch (_gui_initCovarianceInterSysClockDriftUnit)
65 {
66 ✗ case InitCovarianceClockDriftUnits::m_s:
67 ✗ _initCovarianceInterSysClockDrift = std::pow(_gui_initCovarianceInterSysClockDrift, 2);
68 ✗ break;
69 27 case InitCovarianceClockDriftUnits::s_s:
70 27 _initCovarianceInterSysClockDrift = std::pow(_gui_initCovarianceInterSysClockDrift * InsConst::C, 2);
71 27 break;
72 ✗ case InitCovarianceClockDriftUnits::m2_s2:
73 ✗ _initCovarianceInterSysClockDrift = _gui_initCovarianceInterSysClockDrift;
74 ✗ break;
75 ✗ case InitCovarianceClockDriftUnits::s2_s2:
76 ✗ _initCovarianceInterSysClockDrift = _gui_initCovarianceInterSysClockDrift * std::pow(InsConst::C, 2);
77 ✗ break;
78 }
79
80 // ###########################################################################################################
81
82 // Reset values to 0 and remove inter system states
83
1/2
✓ Branch 3 taken 27 times.
✗ Branch 4 not taken.
 54 _kalmanFilter = KeyedKalmanFilterD<States::StateKeyType,
84 27 Meas::MeasKeyTypes>{ PosVelKey, {} };
85
86 27 _motionModel.initialize(_kalmanFilter.F, _kalmanFilter.W);
87
88
2/2
✓ Branch 4 taken 33 times.
✓ Branch 5 taken 27 times.
 60 for (const auto& satSys : satelliteSystems)
89 {
90
1/2
✓ Branch 2 taken 33 times.
✗ Branch 3 not taken.
 33 _kalmanFilter.addState(Keys::RecvClkBias{ satSys });
91
1/2
✓ Branch 2 taken 33 times.
✗ Branch 3 not taken.
 33 _kalmanFilter.addState(Keys::RecvClkDrift{ satSys });
92 }
93 27 _receiverClockModel.initialize(_kalmanFilter.F, _kalmanFilter.G, _kalmanFilter.W);
94
95 LOG_DATA("F = \n{}", _kalmanFilter.F);
96 LOG_DATA("G = \n{}", _kalmanFilter.G);
97 LOG_DATA("W = \n{}", _kalmanFilter.W);
98 27 _initialized = false;
99 27 }
100
101 ✗ void KalmanFilter::initialize(const KeyedVectorXd<States::StateKeyType>& states, const KeyedMatrixXd<States::StateKeyType, States::StateKeyType>& variance)
102 {
103 LOG_DATA("x_KF(pre-init) = \n{}", _kalmanFilter.x.transposed());
104 ✗ _kalmanFilter.x(states.rowKeys()) = states(all);
105 ✗ _kalmanFilter.P(variance.rowKeys(), variance.colKeys()) = variance(all, all) * 100.0; // LSQ variance is very small. So make bigger
106
107 ✗ if (!states.hasAnyRows(VelKey)) // We always estimate velocity in the KF, but LSQ could not, so set a default value
108 {
109 ✗ _kalmanFilter.P(VelKey, VelKey).diagonal() << Eigen::Vector3d::Ones() * _initCovarianceVelocity;
110 }
111 // We always estimate receiver clock drift in the KF, but LSQ could not, so set a default value
112 ✗ for (const auto& key : _kalmanFilter.P.rowKeys())
113 {
114 ✗ if (std::holds_alternative<Keys::RecvClkDrift>(key) && !states.hasRow(key))
115 {
116 ✗ _kalmanFilter.P(key, key) = _initCovarianceClockDrift;
117 }
118 }
119
120 ✗ _initialized = true;
121 ✗ }
122
123 ✗ void KalmanFilter::deinitialize()
124 {
125 ✗ _initialized = false;
126 ✗ }
127
128 ✗ void KalmanFilter::predict(const double& dt, const Eigen::Vector3d& lla_pos, [[maybe_unused]] const std::string& nameId)
129 {
130 // Update the State transition matrix (𝚽) and the Process noise covariance matrix (𝐐)
131
132 LOG_DATA("{}: F =\n{}", nameId, _kalmanFilter.F);
133 LOG_DATA("{}: G =\n{}", nameId, _kalmanFilter.G);
134 LOG_DATA("{}: W =\n{}", nameId, _kalmanFilter.W);
135
136 ✗ _motionModel.updatePhiAndQ(_kalmanFilter.Phi,
137 ✗ _kalmanFilter.Q,
138 ✗ _kalmanFilter.G,
139 ✗ _kalmanFilter.F,
140 ✗ _kalmanFilter.W,
141 dt,
142 ✗ lla_pos(0),
143 ✗ lla_pos(1),
144 _systemModelCalcAlgorithm);
145
146 ✗ _receiverClockModel.updatePhiAndQ(_kalmanFilter.Phi,
147 ✗ _kalmanFilter.Q,
148 ✗ _kalmanFilter.F,
149 ✗ _kalmanFilter.G,
150 ✗ _kalmanFilter.W,
151 dt,
152 _systemModelCalcAlgorithm);
153
154 ✗ _interFrequencyBiasModel.updatePhiAndQ(_kalmanFilter.Phi,
155 ✗ _kalmanFilter.Q,
156 dt);
157
158 LOG_DATA("{}: Phi =\n{}", nameId, _kalmanFilter.Phi);
159 LOG_DATA("{}: Q =\n{}", nameId, _kalmanFilter.Q);
160
161 LOG_DATA("{}: P (a posteriori) =\n{}", nameId, _kalmanFilter.P);
162 LOG_DATA("{}: x (a posteriori) =\n{}", nameId, _kalmanFilter.x.transposed());
163 ✗ _kalmanFilter.predict();
164 LOG_DATA("{}: x (a priori ) =\n{}", nameId, _kalmanFilter.x.transposed());
165 LOG_DATA("{}: P (a priori ) =\n{}", nameId, _kalmanFilter.P);
166 ✗ }
167
168 ✗ void KalmanFilter::update(const std::vector<Meas::MeasKeyTypes>& measKeys,
169 const KeyedMatrixXd<Meas::MeasKeyTypes, States::StateKeyType>& H,
170 const KeyedMatrixXd<Meas::MeasKeyTypes, Meas::MeasKeyTypes>& R,
171 const KeyedVectorXd<Meas::MeasKeyTypes>& dz,
172 [[maybe_unused]] const std::string& nameId)
173 {
174 LOG_DATA("{}: called", nameId);
175
176 ✗ _kalmanFilter.setMeasurements(measKeys);
177
178 ✗ _kalmanFilter.H = H;
179 ✗ _kalmanFilter.R = R;
180 ✗ _kalmanFilter.z = dz;
181
182 ✗ _kalmanFilter.correctWithMeasurementInnovation();
183 ✗ }
184
185 13 void KalmanFilter::addInterFrequencyBias(const Frequency& freq)
186 {
187 13 auto bias = Keys::InterFreqBias{ freq };
188
1/2
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
 13 _kalmanFilter.addState(bias);
189
3/6
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
✓ Branch 11 taken 13 times.
✗ Branch 12 not taken.
 13 _kalmanFilter.P(bias, bias) = _kalmanFilter.P(Keys::RecvClkBias{ freq.getSatSys() },
190
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 13 Keys::RecvClkBias{ freq.getSatSys() });
191 13 _interFrequencyBiasModel.initialize(bias,
192 13 _kalmanFilter.F,
193 13 _kalmanFilter.G,
194
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 13 _kalmanFilter.W);
195 13 }
196
197 ✗ bool KalmanFilter::ShowGuiWidgets(const char* id, bool useDoppler, bool multiConstellation, bool estimateInterFrequencyBiases, float itemWidth, float unitWidth)
198 {
199 ✗ bool changed = false;
200
201 ✗ itemWidth -= ImGui::GetStyle().IndentSpacing;
202 ✗ float configWidth = itemWidth + unitWidth;
203
204 ✗ ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
205 ✗ if (ImGui::TreeNode(fmt::format("System/Process noise##{}", id).c_str()))
206 {
207 ✗ changed |= SystemModelGui(_systemModelCalcAlgorithm, itemWidth, id);
208
209 ✗ changed |= _motionModel.ShowGui(configWidth, unitWidth, id);
210 ✗ changed |= _receiverClockModel.ShowGui(configWidth, unitWidth, id);
211 ✗ if (estimateInterFrequencyBiases)
212 {
213 ✗ changed |= _interFrequencyBiasModel.ShowGui(configWidth, unitWidth, id);
214 }
215
216 ✗ ImGui::TreePop();
217 }
218
219 ✗ if (!useDoppler)
220 {
221 ✗ ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
222 ✗ if (ImGui::TreeNode(fmt::format("Initial Error Covariance Matrix P##{}", id).c_str()))
223 {
224 ✗ if (gui::widgets::InputDoubleWithUnit(fmt::format("{} of the velocity uncertainty##{}",
225 ✗ _gui_initCovarianceVelocityUnit == InitCovarianceVelocityUnits::m_s ? "Standard deviation" : "Variance", id)
226 .c_str(),
227 ✗ configWidth, unitWidth, &_gui_initCovarianceVelocity, _gui_initCovarianceVelocityUnit, "m/s\0m^2/s^2\0\0",
228 0.0, 0.0, "%.2e", ImGuiInputTextFlags_CharsScientific))
229 {
230 ✗ LOG_DEBUG("{}: _gui_initCovarianceVelocity changed to {}", id, _gui_initCovarianceVelocity);
231 ✗ LOG_DEBUG("{}: _gui_initCovarianceVelocityUnit changed to {}", id, fmt::underlying(_gui_initCovarianceVelocityUnit));
232 ✗ changed = true;
233 }
234 ✗ if (gui::widgets::InputDoubleWithUnit(fmt::format("{} of the clock drift uncertainty##{}",
235 ✗ (_gui_initCovarianceClockDriftUnit == InitCovarianceClockDriftUnits::m_s
236 ✗ || _gui_initCovarianceClockDriftUnit == InitCovarianceClockDriftUnits::s_s)
237 ✗ ? "Standard deviation"
238 : "Variance",
239 id)
240 .c_str(),
241 ✗ configWidth, unitWidth, &_gui_initCovarianceClockDrift, _gui_initCovarianceClockDriftUnit, "m/s\0s/s\0m^2/s^2\0s^2/s^2\0\0",
242 0.0, 0.0, "%.2e", ImGuiInputTextFlags_CharsScientific))
243 {
244 ✗ LOG_DEBUG("{}: _gui_initCovarianceClockDrift changed to {}", id, _gui_initCovarianceClockDrift);
245 ✗ LOG_DEBUG("{}: _gui_initCovarianceClockDriftUnit changed to {}", id, fmt::underlying(_gui_initCovarianceClockDriftUnit));
246 ✗ changed = true;
247 }
248 ✗ if (multiConstellation
249 ✗ && gui::widgets::InputDoubleWithUnit(fmt::format("{} of the inter system clock drift uncertainty##{}",
250 ✗ (_gui_initCovarianceInterSysClockDriftUnit == InitCovarianceClockDriftUnits::m_s
251 ✗ || _gui_initCovarianceInterSysClockDriftUnit == InitCovarianceClockDriftUnits::s_s)
252 ✗ ? "Standard deviation"
253 : "Variance",
254 id)
255 .c_str(),
256 ✗ configWidth, unitWidth, &_gui_initCovarianceInterSysClockDrift, _gui_initCovarianceInterSysClockDriftUnit, "m/s\0s/s\0m^2/s^2\0s^2/s^2\0\0",
257 0.0, 0.0, "%.2e", ImGuiInputTextFlags_CharsScientific))
258 {
259 ✗ LOG_DEBUG("{}: _gui_initCovarianceInterSysClockDrift changed to {}", id, _gui_initCovarianceInterSysClockDrift);
260 ✗ LOG_DEBUG("{}: _gui_initCovarianceInterSysClockDriftUnit changed to {}", id, fmt::underlying(_gui_initCovarianceInterSysClockDriftUnit));
261 ✗ changed = true;
262 }
263
264 ✗ ImGui::TreePop();
265 }
266 }
267
268 ✗ ImGui::SetNextItemOpen(false, ImGuiCond_FirstUseEver);
269 ✗ if (ImGui::TreeNode(fmt::format("Kalman Filter matrices##{}", id).c_str()))
270 {
271 ✗ _kalmanFilter.showKalmanFilterMatrixViews(id);
272 ✗ ImGui::TreePop();
273 }
274
275 ✗ return changed;
276 }
277
278 ✗ void KalmanFilter::setClockBiasErrorCovariance(double clkPhaseDrift)
279 {
280 ✗ for (const auto& state : _kalmanFilter.P.rowKeys())
281 {
282 ✗ if (const auto* bias = std::get_if<Keys::RecvClkBias>(&state))
283 {
284 ✗ _kalmanFilter.P(*bias, *bias) = clkPhaseDrift;
285 }
286 }
287 ✗ }
288
289 ✗ const std::vector<SPP::States::StateKeyType>& KalmanFilter::getStateKeys() const
290 {
291 ✗ return _kalmanFilter.x.rowKeys();
292 }
293
294 ✗ const KeyedVectorX<double, States::StateKeyType>& KalmanFilter::getState() const
295 {
296 ✗ return _kalmanFilter.x;
297 }
298
299 ✗ const KeyedMatrixXd<States::StateKeyType, States::StateKeyType>& KalmanFilter::getErrorCovarianceMatrix() const
300 {
301 ✗ return _kalmanFilter.P;
302 }
303
304 ✗ void to_json(json& j, const KalmanFilter& data)
305 {
306 j = {
307 ✗ { "systemModelCalcAlgorithm", data._systemModelCalcAlgorithm },
308 ✗ { "motionModel", data._motionModel },
309 ✗ { "receiverClockModel", data._receiverClockModel },
310 ✗ { "interFrequencyBiasModel", data._interFrequencyBiasModel },
311 ✗ };
312 ✗ } // namespace NAV::SPP
313
314 8 void from_json(const json& j, KalmanFilter& data)
315 {
316
2/2
✓ Branch 1 taken 7 times.
✓ Branch 2 taken 1 times.
 8 if (j.contains("systemModelCalcAlgorithm")) { j.at("systemModelCalcAlgorithm").get_to(data._systemModelCalcAlgorithm); }
317
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 7 times.
 8 if (j.contains("motionModel")) { j.at("motionModel").get_to(data._motionModel); }
318
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 7 times.
 8 if (j.contains("receiverClockModel")) { j.at("receiverClockModel").get_to(data._receiverClockModel); }
319
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 7 times.
 8 if (j.contains("interFrequencyBiasModel")) { j.at("interFrequencyBiasModel").get_to(data._interFrequencyBiasModel); }
320 8 }
321
322 } // namespace NAV::SPP
323