0.4.1
Loading...
Searching...
No Matches
KeyedKalmanFilter.hpp
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/// @file KeyedKalmanFilter.hpp
10/// @brief Kalman Filter with keyed states
11/// @author T. Topp (topp@ins.uni-stuttgart.de)
12/// @date 2023-07-11
13
14#pragma once
15
16#include <span>
17#include <boost/math/distributions/chi_squared.hpp>
18#include <imgui.h>
19
23#include "util/Eigen.hpp"
24#include "util/Json.hpp"
29#include "util/Logger.hpp"
30
31namespace NAV
32{
33/// @brief Keyed Kalman Filter class
34/// @tparam Scalar Numeric type, e.g. float, double, int or std::complex<float>.
35/// @tparam StateKeyType Type of the key used for state lookup
36/// @tparam MeasKeyType Type of the key used for measurement lookup
37template<typename Scalar, typename StateKeyType, typename MeasKeyType>
39{
40 public:
41 /// @brief Default Constructor
42 KeyedKalmanFilter() = default;
43
44 /// @brief Constructor
45 /// @param stateKeys State keys
46 /// @param measKeys Measurement keys
47 KeyedKalmanFilter(std::span<const StateKeyType> stateKeys, std::span<const MeasKeyType> measKeys)
48 {
49 std::unordered_set<StateKeyType> stateSet = { stateKeys.begin(), stateKeys.end() };
50 INS_ASSERT_USER_ERROR(stateSet.size() == stateKeys.size(), "Each state key must be unique");
51 std::unordered_set<MeasKeyType> measSet = { measKeys.begin(), measKeys.end() };
52 INS_ASSERT_USER_ERROR(measSet.size() == measKeys.size(), "Each measurement key must be unique");
53
54 auto n = static_cast<int>(stateKeys.size());
55 auto m = static_cast<int>(measKeys.size());
56
57 x = KeyedVectorX<Scalar, StateKeyType>(Eigen::VectorX<Scalar>::Zero(n), stateKeys);
58 P = KeyedMatrixX<Scalar, StateKeyType, StateKeyType>(Eigen::MatrixX<Scalar>::Zero(n, n), stateKeys);
59 F = KeyedMatrixX<Scalar, StateKeyType, StateKeyType>(Eigen::MatrixX<Scalar>::Zero(n, n), stateKeys);
60 Phi = KeyedMatrixX<Scalar, StateKeyType, StateKeyType>(Eigen::MatrixX<Scalar>::Zero(n, n), stateKeys);
61
62 G = KeyedMatrixX<Scalar, StateKeyType, StateKeyType>(Eigen::MatrixX<Scalar>::Zero(n, n), stateKeys);
63 W = KeyedMatrixX<Scalar, StateKeyType, StateKeyType>(Eigen::MatrixX<Scalar>::Zero(n, n), stateKeys);
64 Q = KeyedMatrixX<Scalar, StateKeyType, StateKeyType>(Eigen::MatrixX<Scalar>::Zero(n, n), stateKeys);
65 z = KeyedVectorX<Scalar, MeasKeyType>(Eigen::VectorX<Scalar>::Zero(m), measKeys);
66 H = KeyedMatrixX<Scalar, MeasKeyType, StateKeyType>(Eigen::MatrixX<Scalar>::Zero(m, n), measKeys, stateKeys);
67 R = KeyedMatrixX<Scalar, MeasKeyType, MeasKeyType>(Eigen::MatrixX<Scalar>::Zero(m, m), measKeys, measKeys);
68 S = KeyedMatrixX<Scalar, MeasKeyType, MeasKeyType>(Eigen::MatrixX<Scalar>::Zero(m, m), measKeys, measKeys);
69 K = KeyedMatrixX<Scalar, StateKeyType, MeasKeyType>(Eigen::MatrixX<Scalar>::Zero(n, m), stateKeys, measKeys);
70 I = Eigen::MatrixX<Scalar>::Identity(n, n);
71 }
72
73 /// @brief Sets all Vectors and matrices to 0
74 void setZero()
75 {
76 x(all).setZero(); // xฬ‚ State vector
77 P(all, all).setZero(); // ๐ Error covariance matrix
78 F(all, all).setZero(); // ๐… System model matrix (n x n)
79 Phi(all, all).setZero(); // ๐šฝ State transition matrix
80 G(all, all).setZero(); // ๐† Noise input matrix (n x o)
81 W(all, all).setZero(); // ๐– Noise scale matrix (o x o)
82 Q(all, all).setZero(); // ๐ System/Process noise covariance matrix
83 z(all).setZero(); // ๐ณ Measurement vector
84 H(all, all).setZero(); // ๐‡ Measurement sensitivity Matrix
85 R(all, all).setZero(); // ๐‘ = ๐ธ{๐ฐโ‚˜๐ฐโ‚˜แต€} Measurement noise covariance matrix
86 S(all, all).setZero(); // ๐—ฆ Measurement prediction covariance matrix
87 K(all, all).setZero(); // ๐Š Kalman gain matrix
88 }
89
90 /// @brief Do a Time Update
91 /// @attention Update the State transition matrix (๐šฝ) and the Process noise covariance matrix (๐) before calling this
92 /// @note See P. Groves (2013) - Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems (ch. 3.2.2)
93 void predict()
94 {
95 // Math: \mathbf{\hat{x}}_k^- = \mathbf{\Phi}_{k-1}\mathbf{\hat{x}}_{k-1}^+ \qquad \text{P. Groves}\,(3.14)
96 x(all) = Phi(all, all) * x(all);
97
98 // Math: \mathbf{P}_k^- = \mathbf{\Phi}_{k-1} P_{k-1}^+ \mathbf{\Phi}_{k-1}^T + \mathbf{Q}_{k-1} \qquad \text{P. Groves}\,(3.15)
99 P(all, all) = Phi(all, all) * P(all, all) * Phi(all, all).transpose() + Q(all, all);
100 }
101
102 /// @brief Do a Measurement Update with a Measurement ๐ณ
103 /// @attention Update the Measurement sensitivity Matrix (๐‡), the Measurement noise covariance matrix (๐‘)
104 /// and the Measurement vector (๐ณ) before calling this
105 /// @note See P. Groves (2013) - Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems (ch. 3.2.2)
106 void correct()
107 {
108 S(all, all) = H(all, all) * P(all, all) * H(all, all).transpose() + R(all, all);
109
110 // Math: \mathbf{K}_k = \mathbf{P}_k^- \mathbf{H}_k^T (\mathbf{H}_k \mathbf{P}_k^- \mathbf{H}_k^T + R_k)^{-1} \qquad \text{P. Groves}\,(3.21)
111 K(all, all) = P(all, all) * H(all, all).transpose() * S(all, all).inverse();
112
113 // Math: \begin{align*} \mathbf{\hat{x}}_k^+ &= \mathbf{\hat{x}}_k^- + \mathbf{K}_k (\mathbf{z}_k - \mathbf{H}_k \mathbf{\hat{x}}_k^-) \\ &= \mathbf{\hat{x}}_k^- + \mathbf{K}_k \mathbf{\delta z}_k^{-} \end{align*} \qquad \text{P. Groves}\,(3.24)
114 x(all) = x(all) + K(all, all) * (z(all) - H(all, all) * x(all));
115
116 // Math: \mathbf{P}_k^+ = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_k^- \qquad \text{P. Groves}\,(3.25)
117 P(all, all) = (I - K(all, all) * H(all, all)) * P(all, all);
118 }
119
120 /// @brief Do a Measurement Update with a Measurement Innovation ๐œน๐ณ
121 /// @attention Update the Measurement sensitivity Matrix (๐‡), the Measurement noise covariance matrix (๐‘)
122 /// and the Measurement vector (๐ณ) before calling this
123 /// @note See P. Groves (2013) - Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems (ch. 3.2.2)
124 /// @note See Brown & Hwang (2012) - Introduction to Random Signals and Applied Kalman Filtering (ch. 5.5 - figure 5.5)
126 {
127 S(all, all) = H(all, all) * P(all, all) * H(all, all).transpose() + R(all, all);
128
129 // Math: \mathbf{K}_k = \mathbf{P}_k^- \mathbf{H}_k^T (\mathbf{H}_k \mathbf{P}_k^- \mathbf{H}_k^T + R_k)^{-1} \qquad \text{P. Groves}\,(3.21)
130 K(all, all) = P(all, all) * H(all, all).transpose() * S(all, all).inverse();
131
132 // Math: \begin{align*} \mathbf{\hat{x}}_k^+ &= \mathbf{\hat{x}}_k^- + \mathbf{K}_k \left(\mathbf{z}_k - \mathbf{h}(\mathbf{\hat{x}}_k^-)\right) \\ &= \mathbf{\hat{x}}_k^- + \mathbf{K}_k \mathbf{\delta z}_k^{-} \end{align*} \qquad \text{P. Groves}\,(3.24)
133 x(all) = x(all) + K(all, all) * z(all);
134
135 // Math: \mathbf{P}_k^+ = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_k^- \qquad \text{P. Groves}\,(3.25)
136 // P(all, all) = (I - K(all, all) * H(all, all)) * P(all, all);
137
138 // Math: \mathbf{P}_k^+ = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_k^- (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k)^T + \mathbf{K}_k \mathbf{R}_k \mathbf{K}_k^T \qquad \text{Brown & Hwang}\,(p. 145, eq. 4.2.11)
139 P(all, all) = (I - K(all, all) * H(all, all)) * P(all, all) * (I - K(all, all) * H(all, all)).transpose() + K(all, all) * R(all, all) * K(all, all).transpose();
140 }
141
142 /// @brief Checks if the filter has the key
143 /// @param stateKey State key
144 [[nodiscard]] bool hasState(const StateKeyType& stateKey) const { return x.hasRow(stateKey); }
145 /// @brief Checks if the filter has the keys
146 /// @param stateKeys State keys
147 [[nodiscard]] bool hasStates(std::span<const StateKeyType> stateKeys) const { return x.hasStates(stateKeys); }
148 /// @brief Checks if the filter has any of the provided keys
149 /// @param stateKeys State keys
150 [[nodiscard]] bool hasAnyStates(std::span<const StateKeyType> stateKeys) const { return x.hasAnyStates(stateKeys); }
151
152 /// @brief Add a new state to the filter
153 /// @param stateKey State key
154 void addState(const StateKeyType& stateKey) { addStates(std::initializer_list<StateKeyType>{ stateKey }); }
155
156 /// @brief Add new states to the filter
157 /// @param stateKeys State keys
158 void addStates(std::span<const StateKeyType> stateKeys)
159 {
160 INS_ASSERT_USER_ERROR(!x.hasAnyRows(stateKeys), "You cannot add a state key which is already in the Kalman filter.");
161 std::unordered_set<StateKeyType> stateSet = { stateKeys.begin(), stateKeys.end() };
162 INS_ASSERT_USER_ERROR(stateSet.size() == stateKeys.size(), "Each state key must be unique");
163
164 auto n = x(all).rows() + static_cast<int>(stateKeys.size());
165
166 x.addRows(stateKeys);
167 P.addRowsCols(stateKeys, stateKeys);
168 F.addRowsCols(stateKeys, stateKeys);
169 Phi.addRowsCols(stateKeys, stateKeys);
170 G.addRowsCols(stateKeys, stateKeys);
171 W.addRowsCols(stateKeys, stateKeys);
172 Q.addRowsCols(stateKeys, stateKeys);
173 H.addCols(stateKeys);
174 K.addRows(stateKeys);
175 I = Eigen::MatrixX<Scalar>::Identity(n, n);
176 }
177
178 /// @brief Remove a state from the filter
179 /// @param stateKey State key
180 void removeState(const StateKeyType& stateKey) { removeStates(std::initializer_list<StateKeyType>{ stateKey }); }
181
182 /// @brief Remove states from the filter
183 /// @param stateKeys State keys
184 void removeStates(std::span<const StateKeyType> stateKeys)
185 {
186 INS_ASSERT_USER_ERROR(x.hasRows(stateKeys), "Not all state keys you are trying to remove are in the Kalman filter.");
187 std::unordered_set<StateKeyType> stateSet = { stateKeys.begin(), stateKeys.end() };
188 INS_ASSERT_USER_ERROR(stateSet.size() == stateKeys.size(), "Each state key must be unique");
189
190 auto n = x.rows() - static_cast<int>(stateKeys.size());
191
192 x.removeRows(stateKeys);
193 P.removeRowsCols(stateKeys, stateKeys);
194 F.removeRowsCols(stateKeys, stateKeys);
195 Phi.removeRowsCols(stateKeys, stateKeys);
196 G.removeRowsCols(stateKeys, stateKeys);
197 W.removeRowsCols(stateKeys, stateKeys);
198 Q.removeRowsCols(stateKeys, stateKeys);
199 H.removeCols(stateKeys);
200 K.removeRows(stateKeys);
201 I = Eigen::MatrixX<Scalar>::Identity(n, n);
202 }
203
204 /// @brief Replace the old with the new key
205 /// @param[in] oldKey Old key to replace
206 /// @param[in] newKey New key to use instead
207 void replaceState(const StateKeyType& oldKey, const StateKeyType& newKey)
208 {
209 x.replaceRowKey(oldKey, newKey);
210 P.replaceRowKey(oldKey, newKey);
211 P.replaceColKey(oldKey, newKey);
212 F.replaceRowKey(oldKey, newKey);
213 F.replaceColKey(oldKey, newKey);
214 Phi.replaceRowKey(oldKey, newKey);
215 Phi.replaceColKey(oldKey, newKey);
216 G.replaceRowKey(oldKey, newKey);
217 G.replaceColKey(oldKey, newKey);
218 W.replaceRowKey(oldKey, newKey);
219 W.replaceColKey(oldKey, newKey);
220 Q.replaceRowKey(oldKey, newKey);
221 Q.replaceColKey(oldKey, newKey);
222 H.replaceColKey(oldKey, newKey);
223 K.replaceRowKey(oldKey, newKey);
224 }
225
226 /// @brief Sets the measurement keys and initializes matrices z, H, R, S, K with Zero
227 /// @param measKeys Measurement keys
228 void setMeasurements(std::span<const MeasKeyType> measKeys)
229 {
230 std::unordered_set<MeasKeyType> measSet = { measKeys.begin(), measKeys.end() };
231 INS_ASSERT_USER_ERROR(measSet.size() == measKeys.size(), "Each measurement key must be unique");
232
233 auto n = static_cast<int>(x.rows());
234 auto m = static_cast<int>(measKeys.size());
235
236 const auto& stateKeys = x.rowKeys();
237
238 z = KeyedVectorX<Scalar, MeasKeyType>(Eigen::VectorX<Scalar>::Zero(m), measKeys);
239 H = KeyedMatrixX<Scalar, MeasKeyType, StateKeyType>(Eigen::MatrixX<Scalar>::Zero(m, n), measKeys, stateKeys);
240 R = KeyedMatrixX<Scalar, MeasKeyType, MeasKeyType>(Eigen::MatrixX<Scalar>::Zero(m, m), measKeys, measKeys);
241 S = KeyedMatrixX<Scalar, MeasKeyType, MeasKeyType>(Eigen::MatrixX<Scalar>::Zero(m, m), measKeys, measKeys);
242 K = KeyedMatrixX<Scalar, StateKeyType, MeasKeyType>(Eigen::MatrixX<Scalar>::Zero(n, m), stateKeys, measKeys);
243 }
244
245 /// @brief Add a measurement to the filter
246 /// @param measKey Measurement key
247 void addMeasurement(const MeasKeyType& measKey) { addMeasurements(std::initializer_list<MeasKeyType>{ measKey }); }
248
249 /// @brief Add measurements to the filter
250 /// @param measKeys Measurement keys
251 void addMeasurements(std::span<const MeasKeyType> measKeys)
252 {
253 INS_ASSERT_USER_ERROR(!z.hasAnyRows(measKeys), "A measurement keys you are trying to add was already in the Kalman filter.");
254 std::unordered_set<MeasKeyType> measurementSet = { measKeys.begin(), measKeys.end() };
255 INS_ASSERT_USER_ERROR(measurementSet.size() == measKeys.size(), "Each measurement key must be unique");
256
257 z.addRows(measKeys);
258 H.addRows(measKeys);
259 R.addRowsCols(measKeys, measKeys);
260 S.addRowsCols(measKeys, measKeys);
261 K.addCols(measKeys);
262 }
263
264 /// @brief Remove a measurement from the filter
265 /// @param measKey Measurement key
266 void removeMeasurement(const MeasKeyType& measKey) { removeMeasurements(std::initializer_list<MeasKeyType>{ measKey }); }
267
268 /// @brief Remove measurements from the filter
269 /// @param measKeys Measurement keys
270 void removeMeasurements(std::span<const MeasKeyType> measKeys)
271 {
272 INS_ASSERT_USER_ERROR(z.hasRows(measKeys), "Not all measurement keys you are trying to remove are in the Kalman filter.");
273 std::unordered_set<MeasKeyType> measurementSet = { measKeys.begin(), measKeys.end() };
274 INS_ASSERT_USER_ERROR(measurementSet.size() == measKeys.size(), "Each measurement key must be unique");
275
276 z.removeRows(measKeys);
277 H.removeRows(measKeys);
278 R.removeRowsCols(measKeys, measKeys);
279 S.removeRowsCols(measKeys, measKeys);
280 K.removeCols(measKeys);
281 }
282
283 KeyedVectorX<Scalar, StateKeyType> x; ///< xฬ‚ State vector (n x 1)
284 KeyedMatrixX<Scalar, StateKeyType, StateKeyType> P; ///< ๐ Error covariance matrix (n x n)
285 KeyedMatrixX<Scalar, StateKeyType, StateKeyType> Phi; ///< ๐šฝ State transition matrix (n x n)
286 KeyedMatrixX<Scalar, StateKeyType, StateKeyType> Q; ///< ๐ System/Process noise covariance matrix (n x n)
287 KeyedVectorX<Scalar, MeasKeyType> z; ///< ๐ณ Measurement vector (m x 1)
288 KeyedMatrixX<Scalar, MeasKeyType, StateKeyType> H; ///< ๐‡ Measurement sensitivity matrix (m x n)
289 KeyedMatrixX<Scalar, MeasKeyType, MeasKeyType> R; ///< ๐‘ = ๐ธ{๐ฐโ‚˜๐ฐโ‚˜แต€} Measurement noise covariance matrix (m x m)
290 KeyedMatrixX<Scalar, MeasKeyType, MeasKeyType> S; ///< ๐—ฆ Measurement prediction covariance matrix (m x m)
291 KeyedMatrixX<Scalar, StateKeyType, MeasKeyType> K; ///< ๐Š Kalman gain matrix (n x m)
292
293 KeyedMatrixX<Scalar, StateKeyType, StateKeyType> F; ///< ๐… System model matrix (n x n)
294 KeyedMatrixX<Scalar, StateKeyType, StateKeyType> G; ///< ๐† Noise input matrix (n x o)
295 KeyedMatrixX<Scalar, StateKeyType, StateKeyType> W; ///< ๐– Noise scale matrix (o x o)
296
297 /// @brief Calculates the state transition matrix ๐šฝ limited to specified order in ๐…๐œโ‚›
298 /// @param[in] tau Time interval in [s]
299 /// @param[in] order The order of the Taylor polynom to calculate
300 /// @note See \cite Groves2013 Groves, ch. 3.2.3, eq. 3.34, p. 98
301 void calcTransitionMatrix_Phi_Taylor(Scalar tau, size_t order)
302 {
303 INS_ASSERT_USER_ERROR(F.rowKeys() == Phi.rowKeys(), "The system model matrix F and the state transition matrix ๐šฝ need to have the same keys.");
304
305 Phi = transitionMatrix_Phi_Taylor(F, tau, order);
306 }
307
308 /// @brief Calculates the state transition matrix ๐šฝ using the exponential matrix
309 /// @param[in] tau Time interval in [s]
310 /// @note See \cite Groves2013 Groves, ch. 3.2.3, eq. 3.33, p. 97
311 /// @attention The cost of the computation is approximately 20n^3 for matrices of size n. The number 20 depends weakly on the norm of the matrix.
313 {
314 INS_ASSERT_USER_ERROR(F.rowKeys() == Phi.rowKeys(), "The system model matrix F and the state transition matrix ๐šฝ need to have the same keys.");
315
317 }
318
319 /// @brief Numerical Method to calculate the State transition matrix ๐šฝ and System/Process noise covariance matrix ๐
320 /// @param[in] dt Time step in [s]
321 /// @note See C.F. van Loan (1978) - Computing Integrals Involving the Matrix Exponential \cite Loan1978
323 {
324 INS_ASSERT_USER_ERROR(G.colKeys() == W.rowKeys(), "The columns of the noise input matrix G and rows of the noise scale matrix W must match. (G * W * G^T)");
325 INS_ASSERT_USER_ERROR(G.rowKeys() == Q.rowKeys(), "The rows of the noise input matrix G and the System/Process noise covariance matrix Q must match.");
326 INS_ASSERT_USER_ERROR(G.colKeys() == Q.colKeys(), "The cols of the noise input matrix G and the System/Process noise covariance matrix Q must match.");
327
328 auto [Phi, Q] = NAV::calcPhiAndQWithVanLoanMethod(F(all, all), G(all, all), W(all, all), dt);
329 this->Phi(all, all) = Phi;
330 this->Q(all, all) = Q;
331 }
332
333 /// @brief Whether a pre-update was saved
334 [[nodiscard]] bool isPreUpdateSaved() const { return _savedPreUpdate.saved; }
335
336 /// @brief Saves xฬ‚, ๐, ๐ณ, ๐‡, ๐‘ a-priori (pre-update)
338 {
339 INS_ASSERT_USER_ERROR(!_savedPreUpdate.saved, "Cannot save the pre-update without restoring or discarding the old one first.");
340 _savedPreUpdate.saved = true;
341
342 _savedPreUpdate.x = x;
343 _savedPreUpdate.P = P;
344 _savedPreUpdate.Phi = Phi;
345 _savedPreUpdate.Q = Q;
346 _savedPreUpdate.z = z;
347 _savedPreUpdate.H = H;
348 _savedPreUpdate.R = R;
349 _savedPreUpdate.S = S;
350 _savedPreUpdate.K = K;
351
352 _savedPreUpdate.F = F;
353 _savedPreUpdate.G = G;
354 _savedPreUpdate.W = W;
355 }
356
357 /// @brief Restores the saved xฬ‚, ๐, ๐ณ, ๐‡, ๐‘ a-priori (pre-update)
359 {
360 INS_ASSERT_USER_ERROR(_savedPreUpdate.saved, "Cannot restore the pre-update without saving one first.");
361 _savedPreUpdate.saved = false;
362
363 x = _savedPreUpdate.x;
364 P = _savedPreUpdate.P;
365 Phi = _savedPreUpdate.Phi;
366 Q = _savedPreUpdate.Q;
367 z = _savedPreUpdate.z;
368 H = _savedPreUpdate.H;
369 R = _savedPreUpdate.R;
370 S = _savedPreUpdate.S;
371 K = _savedPreUpdate.K;
372
373 F = _savedPreUpdate.F;
374 G = _savedPreUpdate.G;
375 W = _savedPreUpdate.W;
376 }
377 /// @brief Discards the saved xฬ‚, ๐, ๐ณ, ๐‡, ๐‘ a-priori (pre-update)
379 {
380 INS_ASSERT_USER_ERROR(_savedPreUpdate.saved, "Cannot discard the pre-update without saving one first.");
381 _savedPreUpdate.saved = false;
382 }
383
384 /// @brief Whether the NIS check should be performed
385 [[nodiscard]] bool isNISenabled() const { return _checkNIS; }
386
387 /// Normalized Innovation Squared (NIS) test results
389 {
390 bool triggered = false; ///< Whether the test triggered
391 double NIS = 0.0; ///< Normalized Innovation Squared (NIS)
392 double r2 = 0.0; ///< Upper boundary of one-sided acceptance interval
393 };
394
395 /// @brief Performs the Normalized Innovation Squared (NIS) test on the measurement innovation ๐œน๐ณ
396 /// @param[in] nameId Caller node for debug output
397 ///
398 /// H_0: The measurement residual ๐œน๐ณ is consistent with the innovation covariance matrix ๐—ฆ
399 /// The acceptance interval is chosen such that the probability that H_0 is accepted is (1 - alpha)
400 /// @return The hypothesis test result if it failed, otherwise nothing.
401 /// @attention Needs to be called before the update
402 [[nodiscard]] auto checkForOutliersNIS([[maybe_unused]] const std::string& nameId)
403 {
404 NISResult ret{};
405 if (z.rows() == 0) { return ret; }
406 S(all, all) = H(all, all) * P(all, all) * H(all, all).transpose() + R(all, all);
407
408 ret.NIS = std::abs(z(all).transpose() * S(all, all).inverse() * z(all));
409
410 boost::math::chi_squared dist(static_cast<double>(z.rows()));
411
412 ret.r2 = boost::math::quantile(dist, 1.0 - _alphaNIS);
413
414 ret.triggered = ret.NIS >= ret.r2;
415
416 if (ret.triggered)
417 {
418 LOG_TRACE("{}: NIS test triggered because: NIS = {:.3f} > {:.3f} = r2", nameId, ret.NIS, ret.r2);
419 }
420 else
421 {
422 LOG_DATA("{}: NIS test passed: NIS = {:.3f} <= {:.3f} = r2", nameId, ret.NIS, ret.r2);
423 }
424
425 return ret;
426 }
427
428 /// @brief Shows GUI elements for the Kalman Filter
429 /// @param[in] id Unique id for ImGui
430 /// @param[in] width Width of the widget
431 /// @return True if something was changed
432 bool showKalmanFilterGUI(const char* id, float width = 0.0F)
433 {
434 bool changed = false;
435
436 changed |= ImGui::Checkbox(fmt::format("Enable outlier NIS check##{}", id).c_str(), &_checkNIS);
437 ImGui::SameLine();
438 gui::widgets::HelpMarker("If the check has too many false positives, try increasing the process noise.");
439
440 if (_checkNIS)
441 {
442 double alpha = _alphaNIS * 100.0;
443 ImGui::SetNextItemWidth(width - ImGui::GetStyle().IndentSpacing);
444 if (ImGui::DragDouble(fmt::format("NIS alpha (failure rate)##{}", id).c_str(), &alpha, 1.0F, 0.0, 100.0, "%.2f %%"))
445 {
446 _alphaNIS = alpha / 100.0;
447 changed = true;
448 }
449 }
450
451 return changed;
452 }
453
454 /// @brief Shows ImGui Tree nodes for all matrices
455 /// @param[in] id Unique id for ImGui
456 /// @param[in] nRows Amount of rows to show
457 void showKalmanFilterMatrixViews(const char* id, int nRows = -2)
458 {
459 ImGui::PushFont(Application::MonoFont());
460 float matrixTableHeight = ImGui::GetTextLineHeightWithSpacing() * static_cast<float>(nRows + 1);
461 float vectorTableHeight = ImGui::GetTextLineHeightWithSpacing() * static_cast<float>(nRows);
462 ImGui::PopFont();
463
464 if (ImGui::TreeNode(fmt::format("x - State vector##{}", id).c_str()))
465 {
466 gui::widgets::KeyedVectorView(fmt::format("Kalman Filter x##{}", id).c_str(), &x, vectorTableHeight);
467 ImGui::TreePop();
468 }
469 if (ImGui::TreeNode(fmt::format("P - Error covariance matrix##{}", id).c_str()))
470 {
471 gui::widgets::KeyedMatrixView(fmt::format("Kalman Filter P##{}", id).c_str(), &P, matrixTableHeight);
472 ImGui::TreePop();
473 }
474 if (ImGui::TreeNode(fmt::format("Phi - State transition matrix##{}", id).c_str()))
475 {
476 gui::widgets::KeyedMatrixView(fmt::format("Kalman Filter Phi##{}", id).c_str(), &Phi, matrixTableHeight);
477 ImGui::TreePop();
478 }
479 if (ImGui::TreeNode(fmt::format("Q System/Process noise covariance matrix##{}", id).c_str()))
480 {
481 gui::widgets::KeyedMatrixView(fmt::format("Kalman Filter Q##{}", id).c_str(), &Q, matrixTableHeight);
482 ImGui::TreePop();
483 }
484 if (ImGui::TreeNode(fmt::format("z - Measurement vector##{}", id).c_str()))
485 {
486 gui::widgets::KeyedVectorView(fmt::format("Kalman Filter z##{}", id).c_str(), &z, vectorTableHeight);
487 ImGui::TreePop();
488 }
489 if (ImGui::TreeNode(fmt::format("H - Measurement sensitivity matrix##{}", id).c_str()))
490 {
491 gui::widgets::KeyedMatrixView(fmt::format("Kalman Filter H##{}", id).c_str(), &H, matrixTableHeight);
492 ImGui::TreePop();
493 }
494 if (ImGui::TreeNode(fmt::format("R - Measurement noise covariance matrix##{}", id).c_str()))
495 {
496 gui::widgets::KeyedMatrixView(fmt::format("Kalman Filter R##{}", id).c_str(), &R, matrixTableHeight);
497 ImGui::TreePop();
498 }
499 if (ImGui::TreeNode(fmt::format("S - Measurement prediction covariance matrix##{}", id).c_str()))
500 {
501 gui::widgets::KeyedMatrixView(fmt::format("Kalman Filter S##{}", id).c_str(), &S, matrixTableHeight);
502 ImGui::TreePop();
503 }
504 if (ImGui::TreeNode(fmt::format("K - Kalman gain matrix##{}", id).c_str()))
505 {
506 gui::widgets::KeyedMatrixView(fmt::format("Kalman Filter K##{}", id).c_str(), &K, matrixTableHeight);
507 ImGui::TreePop();
508 }
509 if (ImGui::TreeNode(fmt::format("F - System model matrix##{}", id).c_str()))
510 {
511 gui::widgets::KeyedMatrixView(fmt::format("Kalman Filter F##{}", id).c_str(), &F, matrixTableHeight);
512 ImGui::TreePop();
513 }
514 if (ImGui::TreeNode(fmt::format("G - Noise input matrix##{}", id).c_str()))
515 {
516 gui::widgets::KeyedMatrixView(fmt::format("Kalman Filter G##{}", id).c_str(), &G, matrixTableHeight);
517 ImGui::TreePop();
518 }
519 if (ImGui::TreeNode(fmt::format("W - Noise scale matrix##{}", id).c_str()))
520 {
521 gui::widgets::KeyedMatrixView(fmt::format("Kalman Filter W##{}", id).c_str(), &W, matrixTableHeight);
522 ImGui::TreePop();
523 }
524 }
525
526 /// @brief Saved pre-update state and measurement
528 {
529 bool saved = false; ///< Flag whether the state was saved
530
531 KeyedVectorX<Scalar, StateKeyType> x; ///< xฬ‚ State vector (n x 1)
532 KeyedMatrixX<Scalar, StateKeyType, StateKeyType> P; ///< ๐ Error covariance matrix (n x n)
533 KeyedMatrixX<Scalar, StateKeyType, StateKeyType> Phi; ///< ๐šฝ State transition matrix (n x n)
534 KeyedMatrixX<Scalar, StateKeyType, StateKeyType> Q; ///< ๐ System/Process noise covariance matrix (n x n)
535 KeyedVectorX<Scalar, MeasKeyType> z; ///< ๐ณ Measurement vector (m x 1)
536 KeyedMatrixX<Scalar, MeasKeyType, StateKeyType> H; ///< ๐‡ Measurement sensitivity matrix (m x n)
537 KeyedMatrixX<Scalar, MeasKeyType, MeasKeyType> R; ///< ๐‘ = ๐ธ{๐ฐโ‚˜๐ฐโ‚˜แต€} Measurement noise covariance matrix (m x m)
538 KeyedMatrixX<Scalar, MeasKeyType, MeasKeyType> S; ///< ๐—ฆ Measurement prediction covariance matrix (m x m)
539 KeyedMatrixX<Scalar, StateKeyType, MeasKeyType> K; ///< ๐Š Kalman gain matrix (n x m)
540 ///
541 KeyedMatrixX<Scalar, StateKeyType, StateKeyType> F; ///< ๐… System model matrix (n x n)
542 KeyedMatrixX<Scalar, StateKeyType, StateKeyType> G; ///< ๐† Noise input matrix (n x o)
543 KeyedMatrixX<Scalar, StateKeyType, StateKeyType> W; ///< ๐– Noise scale matrix (o x o)
544 };
545
546 /// @brief Accesses the saved pre-update matrices
547 [[nodiscard]] const SavedPreUpdate& savedPreUpdate() const { return _savedPreUpdate; }
548
549 private:
550 Eigen::MatrixXd I; ///< ๐‘ฐ Identity matrix (n x n)
551
552 SavedPreUpdate _savedPreUpdate; ///< Saved pre-update state and measurement
553
554 /// @brief Normalized Innovation Squared (NIS) test
555 bool _checkNIS = true;
556
557 /// @brief NIS Test Hypothesis testing failure rate (probability that H_0 is accepted is (1 - alpha))
558 double _alphaNIS = 0.05;
559
560 /// @brief Converts the provided object into json
561 /// @param[out] j Json object which gets filled with the info
562 /// @param[in] obj Object to convert into json
564 {
565 j = json{
566 { "checkNIS", obj._checkNIS },
567 { "alphaNIS", obj._alphaNIS },
568 };
569 }
570 /// @brief Converts the provided json object into a node object
571 /// @param[in] j Json object with the needed values
572 /// @param[out] obj Object to fill from the json
574 {
575 if (j.contains("checkNIS")) { j.at("checkNIS").get_to(obj._checkNIS); }
576 if (j.contains("alphaNIS")) { j.at("alphaNIS").get_to(obj._alphaNIS); }
577 }
578};
579
580/// @brief Keyed Kalman Filter class with double as type
581/// @tparam StateKeyType Type of the key used for state lookup
582/// @tparam MeasKeyType Type of the key used for measurement lookup
583template<typename StateKeyType, typename MeasKeyType>
585
586/// @brief Calculates the state transition matrix ๐šฝ limited to specified order in ๐…๐œโ‚›
587/// @param[in] F System Matrix
588/// @param[in] tau_s time interval in [s]
589/// @param[in] order The order of the Taylor polynom to calculate
590/// @note See \cite Groves2013 Groves, ch. 3.2.3, eq. 3.34, p. 98
591template<typename Scalar, typename RowKeyType, typename ColKeyType, int Rows, int Cols>
593{
594 // Transition matrix ๐šฝ
595 Eigen::Matrix<Scalar, Rows, Cols> Phi;
596
597 if constexpr (Rows == Eigen::Dynamic)
598 {
599 Phi = Eigen::Matrix<Scalar, Rows, Cols>::Identity(F.rows(), F.cols());
600 }
601 else
602 {
603 Phi = Eigen::Matrix<Scalar, Rows, Cols>::Identity();
604 }
605 // std::cout << "Phi = I";
606
607 for (size_t i = 1; i <= order; i++)
608 {
609 Eigen::Matrix<Scalar, Rows, Cols> Fpower = F(all, all);
610 // std::cout << " + (F";
611
612 for (size_t j = 1; j < i; j++) // F^j
613 {
614 // std::cout << "*F";
615 Fpower *= F(all, all);
616 }
617 // std::cout << "*tau_s^" << i << ")";
618 // std::cout << "/" << math::factorial(i);
619 Phi += (Fpower * std::pow(tau_s, i)) / math::factorial(i);
620 }
621 // std::cout << "\n";
622
623 return { Phi, F.rowKeys(), F.colKeys() };
624}
625
626/// @brief Calculates the state transition matrix ๐šฝ using the exponential matrix
627/// @param[in] F System Matrix
628/// @param[in] tau_s time interval in [s]
629/// @note See \cite Groves2013 Groves, ch. 3.2.3, eq. 3.33, p. 97
630/// @attention The cost of the computation is approximately 20n^3 for matrices of size n. The number 20 depends weakly on the norm of the matrix.
631template<typename Scalar, typename RowKeyType, typename ColKeyType, int Rows, int Cols>
633{
634 // Transition matrix ๐šฝ
635 return { (F(all, all) * tau_s).exp(), F.rowKeys(), F.colKeys() };
636}
637
638} // namespace NAV
#define INS_ASSERT_USER_ERROR(_EXP, _MSG)
Assert function with message.
Definition Assert.h:21
Vector space operations.
nlohmann::json json
json namespace
Text Help Marker (?) with Tooltip.
The class is responsible for all time-related tasks.
Defines how to save certain datatypes to json.
Utility class for logging to console and file.
#define LOG_DATA
All output which occurs repeatedly every time observations are received.
Definition Logger.hpp:29
#define LOG_TRACE
Detailled info to trace the execution of the program. Should not be called on functions which receive...
Definition Logger.hpp:65
Simple Math functions.
Van Loan Method loan1978.
Keyed Kalman Filter class.
KeyedMatrixX< double, MeasKeyType, StateKeyType > H
bool showKalmanFilterGUI(const char *id, float width=0.0F)
Shows GUI elements for the Kalman Filter.
void calcPhiAndQWithVanLoanMethod(Scalar dt)
Numerical Method to calculate the State transition matrix ๐šฝ and System/Process noise covariance matri...
void replaceState(const StateKeyType &oldKey, const StateKeyType &newKey)
Replace the old with the new key.
void addStates(std::span< const StateKeyType > stateKeys)
Add new states to the filter.
void discardPreUpdate()
Discards the saved xฬ‚, ๐, ๐ณ, ๐‡, ๐‘ a-priori (pre-update)
bool isPreUpdateSaved() const
Whether a pre-update was saved.
bool hasState(const StateKeyType &stateKey) const
Checks if the filter has the key.
void removeState(const StateKeyType &stateKey)
Remove a state from the filter.
friend void from_json(const json &j, KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType > &obj)
Converts the provided json object into a node object.
auto checkForOutliersNIS(const std::string &nameId)
Performs the Normalized Innovation Squared (NIS) test on the measurement innovation ๐œน๐ณ
void removeMeasurements(std::span< const MeasKeyType > measKeys)
Remove measurements from the filter.
KeyedMatrixX< double, MeasKeyType, MeasKeyType > S
KeyedMatrixX< double, StateKeyType, MeasKeyType > K
friend void to_json(json &j, const KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType > &obj)
Converts the provided object into json.
KeyedKalmanFilter(std::span< const StateKeyType > stateKeys, std::span< const MeasKeyType > measKeys)
Constructor.
KeyedMatrixX< double, StateKeyType, StateKeyType > W
void addMeasurement(const MeasKeyType &measKey)
Add a measurement to the filter.
void removeStates(std::span< const StateKeyType > stateKeys)
Remove states from the filter.
KeyedMatrixX< double, StateKeyType, StateKeyType > F
void removeMeasurement(const MeasKeyType &measKey)
Remove a measurement from the filter.
void showKalmanFilterMatrixViews(const char *id, int nRows=-2)
Shows ImGui Tree nodes for all matrices.
KeyedMatrixX< double, StateKeyType, StateKeyType > Q
void addState(const StateKeyType &stateKey)
Add a new state to the filter.
void restorePreUpdate()
Restores the saved xฬ‚, ๐, ๐ณ, ๐‡, ๐‘ a-priori (pre-update)
void setZero()
Sets all Vectors and matrices to 0.
bool hasStates(std::span< const StateKeyType > stateKeys) const
Checks if the filter has the keys.
bool hasAnyStates(std::span< const StateKeyType > stateKeys) const
Checks if the filter has any of the provided keys.
void correctWithMeasurementInnovation()
Do a Measurement Update with a Measurement Innovation ๐œน๐ณ
void predict()
Do a Time Update.
bool isNISenabled() const
Whether the NIS check should be performed.
void calcTransitionMatrix_Phi_exp(Scalar tau)
Calculates the state transition matrix ๐šฝ using the exponential matrix.
void calcTransitionMatrix_Phi_Taylor(Scalar tau, size_t order)
Calculates the state transition matrix ๐šฝ limited to specified order in ๐…๐œโ‚›
void savePreUpdate()
Saves xฬ‚, ๐, ๐ณ, ๐‡, ๐‘ a-priori (pre-update)
void correct()
Do a Measurement Update with a Measurement ๐ณ
KeyedMatrixX< double, StateKeyType, StateKeyType > Phi
KeyedKalmanFilter()=default
Default Constructor.
KeyedMatrixX< double, MeasKeyType, MeasKeyType > R
void addMeasurements(std::span< const MeasKeyType > measKeys)
Add measurements to the filter.
void setMeasurements(std::span< const MeasKeyType > measKeys)
Sets the measurement keys and initializes matrices z, H, R, S, K with Zero.
KeyedMatrixX< double, StateKeyType, StateKeyType > P
const SavedPreUpdate & savedPreUpdate() const
Accesses the saved pre-update matrices.
KeyedMatrixX< double, StateKeyType, StateKeyType > G
Static sized KeyedMatrix.
const std::vector< ColKeyType > & colKeys() const
Returns the col keys.
decltype(auto) cols() const
Return the cols of the underlying Eigen matrix.
decltype(auto) rows() const
Return the rows of the underlying Eigen matrix.
const std::vector< RowKeyType > & rowKeys() const
Returns the row keys.
ImGui extensions.
Widgets related to KeyedMatrices.
bool DragDouble(const char *label, double *v, float v_speed, double v_min, double v_max, const char *format, ImGuiSliderFlags flags)
Shows a Drag GUI element for 'double'.
Definition imgui_ex.cpp:19
void KeyedVectorView(const char *label, const KeyedVector< Scalar, RowKeyType, Rows > *matrix, float tableHeight=-1.0F, ImGuiTableFlags tableFlags=ImGuiTableFlags_Borders|ImGuiTableFlags_NoHostExtendX|ImGuiTableFlags_SizingFixedFit|ImGuiTableFlags_ScrollY)
Shows GUI elements to display the coefficients of a matrix.
void KeyedMatrixView(const char *label, const KeyedMatrix< Scalar, RowKeyType, ColKeyType, Rows, Cols > *matrix, float tableHeight=-1.0F, ImGuiTableFlags tableFlags=ImGuiTableFlags_Borders|ImGuiTableFlags_NoHostExtendX|ImGuiTableFlags_SizingFixedFit|ImGuiTableFlags_ScrollX|ImGuiTableFlags_ScrollY)
Shows GUI elements to display the coefficients of a matrix.
void HelpMarker(const char *desc, const char *symbol="(?)")
Text Help Marker, e.g. '(?)', with Tooltip.
uint64_t factorial(uint64_t n)
Calculates the factorial of an unsigned integer.
Definition Math.cpp:14
std::pair< typename DerivedF::PlainObject, typename DerivedF::PlainObject > calcPhiAndQWithVanLoanMethod(const Eigen::MatrixBase< DerivedF > &F, const Eigen::MatrixBase< DerivedG > &G, const Eigen::MatrixBase< DerivedW > &W, double dt)
Numerical Method to calculate the State transition matrix ๐šฝ and System/Process noise covariance matri...
Definition VanLoan.hpp:65
Derived::PlainObject transitionMatrix_Phi_exp(const Eigen::MatrixBase< Derived > &F, typename Derived::Scalar tau_s)
Calculates the state transition matrix ๐šฝ using the exponential matrix.
KeyedMatrix< Scalar, RowKeyType, ColKeyType, Eigen::Dynamic, Eigen::Dynamic > KeyedMatrixX
Dynamic size KeyedMatrix.
Derived::PlainObject transitionMatrix_Phi_Taylor(const Eigen::MatrixBase< Derived > &F, double tau_s, size_t order)
Calculates the state transition matrix ๐šฝ limited to specified order in ๐…๐œโ‚›
KeyedKalmanFilter< double, StateKeyType, MeasKeyType > KeyedKalmanFilterD
Keyed Kalman Filter class with double as type.
KeyedVector< Scalar, RowKeyType, Eigen::Dynamic > KeyedVectorX
Dynamic size KeyedVector.
static const internal::all_t all
Used to request all rows or columns in KeyedMatrices.
Normalized Innovation Squared (NIS) test results.
double NIS
Normalized Innovation Squared (NIS)
bool triggered
Whether the test triggered.
double r2
Upper boundary of one-sided acceptance interval.
Saved pre-update state and measurement.
KeyedMatrixX< Scalar, StateKeyType, StateKeyType > F
๐… System model matrix (n x n)
KeyedMatrixX< Scalar, MeasKeyType, StateKeyType > H
๐‡ Measurement sensitivity matrix (m x n)
KeyedMatrixX< Scalar, StateKeyType, StateKeyType > Q
๐ System/Process noise covariance matrix (n x n)
KeyedMatrixX< Scalar, StateKeyType, StateKeyType > G
๐† Noise input matrix (n x o)
KeyedMatrixX< Scalar, StateKeyType, StateKeyType > P
๐ Error covariance matrix (n x n)
KeyedMatrixX< Scalar, StateKeyType, MeasKeyType > K
KeyedMatrixX< Scalar, StateKeyType, StateKeyType > Phi
๐šฝ State transition matrix (n x n)
bool saved
Flag whether the state was saved.
KeyedVectorX< Scalar, MeasKeyType > z
๐ณ Measurement vector (m x 1)
KeyedMatrixX< Scalar, MeasKeyType, MeasKeyType > R
๐‘ = ๐ธ{๐ฐโ‚˜๐ฐโ‚˜แต€} Measurement noise covariance matrix (m x m)
KeyedMatrixX< Scalar, StateKeyType, StateKeyType > W
๐– Noise scale matrix (o x o)
KeyedVectorX< Scalar, StateKeyType > x
xฬ‚ State vector (n x 1)
KeyedMatrixX< Scalar, MeasKeyType, MeasKeyType > S
๐—ฆ Measurement prediction covariance matrix (m x m)
Matrix which can be accessed by keys.