INSTINCT Code Coverage Report

Directory: src/
File: Navigation/Math/KeyedKalmanFilter.hpp
Date: 2025-06-02 15:19:59
Exec Total Coverage
Lines: 143 185 77.3%
Functions: 23 28 82.1%
Branches: 155 378 41.0%
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 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
20 #include "internal/gui/widgets/HelpMarker.hpp"
21 #include "internal/gui/widgets/imgui_ex.hpp"
22 #include "internal/gui/widgets/KeyedMatrix.hpp"
23 #include "util/Eigen.hpp"
24 #include "util/Json.hpp"
25 #include "util/Container/KeyedMatrix.hpp"
26 #include "Navigation/Time/InsTime.hpp"
27 #include "Navigation/Math/Math.hpp"
28 #include "Navigation/Math/VanLoan.hpp"
29 #include "util/Logger.hpp"
30
31 namespace 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
37 template<typename Scalar, typename StateKeyType, typename MeasKeyType>
38 class KeyedKalmanFilter
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 271 KeyedKalmanFilter(std::span<const StateKeyType> stateKeys, std::span<const MeasKeyType> measKeys)
48 271 {
49
1/2
✓ Branch 3 taken 271 times.
✗ Branch 4 not taken.
 542 std::unordered_set<StateKeyType> stateSet = { stateKeys.begin(), stateKeys.end() };
50
1/2
✓ Branch 2 taken 271 times.
✗ Branch 3 not taken.
 271 INS_ASSERT_USER_ERROR(stateSet.size() == stateKeys.size(), "Each state key must be unique");
51
1/2
✓ Branch 3 taken 271 times.
✗ Branch 4 not taken.
 542 std::unordered_set<MeasKeyType> measSet = { measKeys.begin(), measKeys.end() };
52
1/2
✓ Branch 2 taken 271 times.
✗ Branch 3 not taken.
 271 INS_ASSERT_USER_ERROR(measSet.size() == measKeys.size(), "Each measurement key must be unique");
53
54 271 auto n = static_cast<int>(stateKeys.size());
55 271 auto m = static_cast<int>(measKeys.size());
56
57
2/4
✓ Branch 1 taken 271 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 271 times.
✗ Branch 5 not taken.
 271 x = KeyedVectorX<Scalar, StateKeyType>(Eigen::VectorX<Scalar>::Zero(n), stateKeys);
58
2/4
✓ Branch 1 taken 271 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 271 times.
✗ Branch 5 not taken.
 271 P = KeyedMatrixX<Scalar, StateKeyType, StateKeyType>(Eigen::MatrixX<Scalar>::Zero(n, n), stateKeys);
59
2/4
✓ Branch 1 taken 271 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 271 times.
✗ Branch 5 not taken.
 271 F = KeyedMatrixX<Scalar, StateKeyType, StateKeyType>(Eigen::MatrixX<Scalar>::Zero(n, n), stateKeys);
60
2/4
✓ Branch 1 taken 271 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 271 times.
✗ Branch 5 not taken.
 271 Phi = KeyedMatrixX<Scalar, StateKeyType, StateKeyType>(Eigen::MatrixX<Scalar>::Zero(n, n), stateKeys);
61
62
2/4
✓ Branch 1 taken 271 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 271 times.
✗ Branch 5 not taken.
 271 G = KeyedMatrixX<Scalar, StateKeyType, StateKeyType>(Eigen::MatrixX<Scalar>::Zero(n, n), stateKeys);
63
2/4
✓ Branch 1 taken 271 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 271 times.
✗ Branch 5 not taken.
 271 W = KeyedMatrixX<Scalar, StateKeyType, StateKeyType>(Eigen::MatrixX<Scalar>::Zero(n, n), stateKeys);
64
2/4
✓ Branch 1 taken 271 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 271 times.
✗ Branch 5 not taken.
 271 Q = KeyedMatrixX<Scalar, StateKeyType, StateKeyType>(Eigen::MatrixX<Scalar>::Zero(n, n), stateKeys);
65
2/4
✓ Branch 1 taken 271 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 271 times.
✗ Branch 5 not taken.
 271 z = KeyedVectorX<Scalar, MeasKeyType>(Eigen::VectorX<Scalar>::Zero(m), measKeys);
66
2/4
✓ Branch 1 taken 271 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 271 times.
✗ Branch 5 not taken.
 271 H = KeyedMatrixX<Scalar, MeasKeyType, StateKeyType>(Eigen::MatrixX<Scalar>::Zero(m, n), measKeys, stateKeys);
67
2/4
✓ Branch 1 taken 271 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 271 times.
✗ Branch 5 not taken.
 271 R = KeyedMatrixX<Scalar, MeasKeyType, MeasKeyType>(Eigen::MatrixX<Scalar>::Zero(m, m), measKeys, measKeys);
68
2/4
✓ Branch 1 taken 271 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 271 times.
✗ Branch 5 not taken.
 271 S = KeyedMatrixX<Scalar, MeasKeyType, MeasKeyType>(Eigen::MatrixX<Scalar>::Zero(m, m), measKeys, measKeys);
69
2/4
✓ Branch 1 taken 271 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 271 times.
✗ Branch 5 not taken.
 271 K = KeyedMatrixX<Scalar, StateKeyType, MeasKeyType>(Eigen::MatrixX<Scalar>::Zero(n, m), stateKeys, measKeys);
70
2/4
✓ Branch 1 taken 271 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 271 times.
✗ Branch 5 not taken.
 271 I = Eigen::MatrixX<Scalar>::Identity(n, n);
71 271 }
72
73 /// @brief Sets all Vectors and matrices to 0
74 51 void setZero()
75 {
76 51 x(all).setZero(); // xฬ‚ State vector
77 51 P(all, all).setZero(); // ๐ Error covariance matrix
78 51 F(all, all).setZero(); // ๐… System model matrix (n x n)
79 51 Phi(all, all).setZero(); // ๐šฝ State transition matrix
80 51 G(all, all).setZero(); // ๐† Noise input matrix (n x o)
81 51 W(all, all).setZero(); // ๐– Noise scale matrix (o x o)
82 51 Q(all, all).setZero(); // ๐ System/Process noise covariance matrix
83 51 z(all).setZero(); // ๐ณ Measurement vector
84 51 H(all, all).setZero(); // ๐‡ Measurement sensitivity Matrix
85 51 R(all, all).setZero(); // ๐‘ = ๐ธ{๐ฐโ‚˜๐ฐโ‚˜แต€} Measurement noise covariance matrix
86 51 S(all, all).setZero(); // ๐—ฆ Measurement prediction covariance matrix
87 51 K(all, all).setZero(); // ๐Š Kalman gain matrix
88 51 }
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 49998 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
2/4
✓ Branch 3 taken 49998 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 49998 times.
✗ Branch 8 not taken.
 49998 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
5/10
✓ Branch 3 taken 49998 times.
✗ Branch 4 not taken.
✓ Branch 8 taken 49998 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 49998 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 49998 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 49998 times.
✗ Branch 19 not taken.
 49998 P(all, all) = Phi(all, all) * P(all, all) * Phi(all, all).transpose() + Q(all, all);
100 49998 }
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 1 void correct()
107 {
108
5/10
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
 1 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
5/10
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
 1 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
5/10
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
 1 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
4/8
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
 1 P(all, all) = (I - K(all, all) * H(all, all)) * P(all, all);
118 1 }
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)
125 26814 void correctWithMeasurementInnovation()
126 {
127
5/10
✓ Branch 3 taken 26814 times.
✗ Branch 4 not taken.
✓ Branch 8 taken 26814 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 26814 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 26814 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 26814 times.
✗ Branch 19 not taken.
 26814 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
5/10
✓ Branch 2 taken 26814 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 26814 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 26814 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 26814 times.
✗ Branch 14 not taken.
✓ Branch 17 taken 26814 times.
✗ Branch 18 not taken.
 26814 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
3/6
✓ Branch 3 taken 26814 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 26814 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 26814 times.
✗ Branch 12 not taken.
 26814 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
12/24
✓ Branch 2 taken 26814 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 26814 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 26814 times.
✗ Branch 11 not taken.
✓ Branch 15 taken 26814 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 26814 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 26814 times.
✗ Branch 22 not taken.
✓ Branch 27 taken 26814 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 26814 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 26814 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 26814 times.
✗ Branch 37 not taken.
✓ Branch 39 taken 26814 times.
✗ Branch 40 not taken.
✓ Branch 43 taken 26814 times.
✗ Branch 44 not taken.
 26814 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 26814 }
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
1/2
✓ Branch 2 taken 51 times.
✗ Branch 3 not taken.
 51 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 53 void addStates(std::span<const StateKeyType> stateKeys)
159 {
160
2/4
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 53 times.
✗ Branch 4 not taken.
 53 INS_ASSERT_USER_ERROR(!x.hasAnyRows(stateKeys), "You cannot add a state key which is already in the Kalman filter.");
161
1/2
✓ Branch 3 taken 53 times.
✗ Branch 4 not taken.
 106 std::unordered_set<StateKeyType> stateSet = { stateKeys.begin(), stateKeys.end() };
162
1/2
✓ Branch 2 taken 53 times.
✗ Branch 3 not taken.
 53 INS_ASSERT_USER_ERROR(stateSet.size() == stateKeys.size(), "Each state key must be unique");
163
164 53 auto n = x(all).rows() + static_cast<int>(stateKeys.size());
165
166
1/2
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
 53 x.addRows(stateKeys);
167
1/2
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
 53 P.addRowsCols(stateKeys, stateKeys);
168
1/2
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
 53 F.addRowsCols(stateKeys, stateKeys);
169
1/2
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
 53 Phi.addRowsCols(stateKeys, stateKeys);
170
1/2
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
 53 G.addRowsCols(stateKeys, stateKeys);
171
1/2
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
 53 W.addRowsCols(stateKeys, stateKeys);
172
1/2
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
 53 Q.addRowsCols(stateKeys, stateKeys);
173
1/2
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
 53 H.addCols(stateKeys);
174
1/2
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
 53 K.addRows(stateKeys);
175
2/4
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 53 times.
✗ Branch 5 not taken.
 53 I = Eigen::MatrixX<Scalar>::Identity(n, n);
176 53 }
177
178 /// @brief Remove a state from the filter
179 /// @param stateKey State key
180
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 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 2 void removeStates(std::span<const StateKeyType> stateKeys)
185 {
186
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 2 INS_ASSERT_USER_ERROR(x.hasRows(stateKeys), "Not all state keys you are trying to remove are in the Kalman filter.");
187
1/2
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 4 std::unordered_set<StateKeyType> stateSet = { stateKeys.begin(), stateKeys.end() };
188
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 INS_ASSERT_USER_ERROR(stateSet.size() == stateKeys.size(), "Each state key must be unique");
189
190 2 auto n = x.rows() - static_cast<int>(stateKeys.size());
191
192
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 x.removeRows(stateKeys);
193
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 P.removeRowsCols(stateKeys, stateKeys);
194
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 F.removeRowsCols(stateKeys, stateKeys);
195
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Phi.removeRowsCols(stateKeys, stateKeys);
196
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 G.removeRowsCols(stateKeys, stateKeys);
197
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 W.removeRowsCols(stateKeys, stateKeys);
198
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Q.removeRowsCols(stateKeys, stateKeys);
199
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 H.removeCols(stateKeys);
200
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 K.removeRows(stateKeys);
201
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 I = Eigen::MatrixX<Scalar>::Identity(n, n);
202 2 }
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 1 void replaceState(const StateKeyType& oldKey, const StateKeyType& newKey)
208 {
209 1 x.replaceRowKey(oldKey, newKey);
210 1 P.replaceRowKey(oldKey, newKey);
211 1 P.replaceColKey(oldKey, newKey);
212 1 F.replaceRowKey(oldKey, newKey);
213 1 F.replaceColKey(oldKey, newKey);
214 1 Phi.replaceRowKey(oldKey, newKey);
215 1 Phi.replaceColKey(oldKey, newKey);
216 1 G.replaceRowKey(oldKey, newKey);
217 1 G.replaceColKey(oldKey, newKey);
218 1 W.replaceRowKey(oldKey, newKey);
219 1 W.replaceColKey(oldKey, newKey);
220 1 Q.replaceRowKey(oldKey, newKey);
221 1 Q.replaceColKey(oldKey, newKey);
222 1 H.replaceColKey(oldKey, newKey);
223 1 K.replaceRowKey(oldKey, newKey);
224 1 }
225
226 /// @brief Sets the measurement keys and initializes matrices z, H, R, S, K with Zero
227 /// @param measKeys Measurement keys
228 26815 void setMeasurements(std::span<const MeasKeyType> measKeys)
229 {
230
1/2
✓ Branch 3 taken 26815 times.
✗ Branch 4 not taken.
 53630 std::unordered_set<MeasKeyType> measSet = { measKeys.begin(), measKeys.end() };
231
1/2
✓ Branch 2 taken 26815 times.
✗ Branch 3 not taken.
 26815 INS_ASSERT_USER_ERROR(measSet.size() == measKeys.size(), "Each measurement key must be unique");
232
233 26815 auto n = static_cast<int>(x.rows());
234 26815 auto m = static_cast<int>(measKeys.size());
235
236 26815 const auto& stateKeys = x.rowKeys();
237
238
2/4
✓ Branch 1 taken 26815 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26815 times.
✗ Branch 5 not taken.
 26815 z = KeyedVectorX<Scalar, MeasKeyType>(Eigen::VectorX<Scalar>::Zero(m), measKeys);
239
2/4
✓ Branch 2 taken 26815 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 26815 times.
✗ Branch 6 not taken.
 26815 H = KeyedMatrixX<Scalar, MeasKeyType, StateKeyType>(Eigen::MatrixX<Scalar>::Zero(m, n), measKeys, stateKeys);
240
2/4
✓ Branch 1 taken 26815 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26815 times.
✗ Branch 5 not taken.
 26815 R = KeyedMatrixX<Scalar, MeasKeyType, MeasKeyType>(Eigen::MatrixX<Scalar>::Zero(m, m), measKeys, measKeys);
241
2/4
✓ Branch 1 taken 26815 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26815 times.
✗ Branch 5 not taken.
 26815 S = KeyedMatrixX<Scalar, MeasKeyType, MeasKeyType>(Eigen::MatrixX<Scalar>::Zero(m, m), measKeys, measKeys);
242
2/4
✓ Branch 2 taken 26815 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 26815 times.
✗ Branch 6 not taken.
 26815 K = KeyedMatrixX<Scalar, StateKeyType, MeasKeyType>(Eigen::MatrixX<Scalar>::Zero(n, m), stateKeys, measKeys);
243 26815 }
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 12888 void calcTransitionMatrix_Phi_Taylor(Scalar tau, size_t order)
302 {
303
1/2
✓ Branch 3 taken 12888 times.
✗ Branch 4 not taken.
 12888 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
1/2
✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
 12888 Phi = transitionMatrix_Phi_Taylor(F, tau, order);
306 12888 }
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.
312 6444 void calcTransitionMatrix_Phi_exp(Scalar tau)
313 {
314
1/2
✓ Branch 3 taken 6444 times.
✗ Branch 4 not taken.
 6444 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
316
1/2
✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
 6444 Phi = transitionMatrix_Phi_exp(F, tau);
317 6444 }
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
322 37109 void calcPhiAndQWithVanLoanMethod(Scalar dt)
323 {
324
2/4
✓ Branch 3 taken 37109 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 37109 times.
✗ Branch 6 not taken.
 37109 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
2/4
✓ Branch 3 taken 37109 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 37109 times.
✗ Branch 6 not taken.
 37109 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
2/4
✓ Branch 3 taken 37109 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 37109 times.
✗ Branch 6 not taken.
 37109 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
1/2
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.
 37109 auto [Phi, Q] = NAV::calcPhiAndQWithVanLoanMethod(F(all, all), G(all, all), W(all, all), dt);
329
1/2
✓ Branch 2 taken 37109 times.
✗ Branch 3 not taken.
 37109 this->Phi(all, all) = Phi;
330
1/2
✓ Branch 2 taken 37109 times.
✗ Branch 3 not taken.
 37109 this->Q(all, all) = Q;
331 37109 }
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)
337 void savePreUpdate()
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)
358 void restorePreUpdate()
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)
378 void discardPreUpdate()
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
388 struct NISResult
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
527 struct SavedPreUpdate
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
563 friend void to_json(json& j, const KeyedKalmanFilter<Scalar, StateKeyType, MeasKeyType>& obj)
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
573 friend void from_json(const json& j, KeyedKalmanFilter<Scalar, StateKeyType, MeasKeyType>& obj)
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
583 template<typename StateKeyType, typename MeasKeyType>
584 using KeyedKalmanFilterD = KeyedKalmanFilter<double, StateKeyType, 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
591 template<typename Scalar, typename RowKeyType, typename ColKeyType, int Rows, int Cols>
592 12888 KeyedMatrix<Scalar, RowKeyType, ColKeyType, Rows, Cols> transitionMatrix_Phi_Taylor(const KeyedMatrix<Scalar, RowKeyType, ColKeyType, Rows, Cols>& F, Scalar tau_s, size_t order)
593 {
594 // Transition matrix ๐šฝ
595
1/2
✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
 12888 Eigen::Matrix<Scalar, Rows, Cols> Phi;
596
597 if constexpr (Rows == Eigen::Dynamic)
598 {
599
2/4
✓ Branch 3 taken 12888 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 12888 times.
✗ Branch 7 not taken.
 12888 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
2/2
✓ Branch 0 taken 25776 times.
✓ Branch 1 taken 12888 times.
 38664 for (size_t i = 1; i <= order; i++)
608 {
609
1/2
✓ Branch 2 taken 25776 times.
✗ Branch 3 not taken.
 25776 Eigen::Matrix<Scalar, Rows, Cols> Fpower = F(all, all);
610 // std::cout << " + (F";
611
612
2/2
✓ Branch 0 taken 12888 times.
✓ Branch 1 taken 25776 times.
 38664 for (size_t j = 1; j < i; j++) // F^j
613 {
614 // std::cout << "*F";
615
1/2
✓ Branch 2 taken 12888 times.
✗ Branch 3 not taken.
 12888 Fpower *= F(all, all);
616 }
617 // std::cout << "*tau_s^" << i << ")";
618 // std::cout << "/" << math::factorial(i);
619
4/8
✓ Branch 1 taken 25776 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 25776 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 25776 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 25776 times.
✗ Branch 12 not taken.
 25776 Phi += (Fpower * std::pow(tau_s, i)) / math::factorial(i);
620 }
621 // std::cout << "\n";
622
623
1/2
✓ Branch 5 taken 12888 times.
✗ Branch 6 not taken.
 25776 return { Phi, F.rowKeys(), F.colKeys() };
624 12888 }
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.
631 template<typename Scalar, typename RowKeyType, typename ColKeyType, int Rows, int Cols>
632 6444 KeyedMatrix<Scalar, RowKeyType, ColKeyType, Rows, Cols> transitionMatrix_Phi_exp(const KeyedMatrix<Scalar, RowKeyType, ColKeyType, Rows, Cols>& F, Scalar tau_s)
633 {
634 // Transition matrix ๐šฝ
635
3/6
✓ Branch 2 taken 6444 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6444 times.
✗ Branch 6 not taken.
✓ Branch 12 taken 6444 times.
✗ Branch 13 not taken.
 6444 return { (F(all, all) * tau_s).exp(), F.rowKeys(), F.colKeys() };
636 }
637
638 } // namespace NAV
639