INSTINCT Code Coverage Report

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