mrs_lib
Various reusable classes, functions and utilities for use in MRS projects
Loading...
Searching...
No Matches
lkf.h
1// clang: MatousFormat
6#ifndef LKFSYSTEMMODELS_H
7#define LKFSYSTEMMODELS_H
8
10#include <iostream>
11
12namespace mrs_lib
13{
14
15 /* class LKF //{ */
37 template <int n_states, int n_inputs, int n_measurements>
38 class LKF : public KalmanFilter<n_states, n_inputs, n_measurements>
39 {
40 public:
41 /* LKF definitions (typedefs, constants etc) //{ */
42 static constexpr int n = n_states;
43 static constexpr int m = n_inputs;
44 static constexpr int p = n_measurements;
47 using x_t = typename Base_class::x_t;
48 using u_t = typename Base_class::u_t;
49 using z_t = typename Base_class::z_t;
50 using P_t = typename Base_class::P_t;
51 using R_t = typename Base_class::R_t;
52 using Q_t = typename Base_class::Q_t;
55 typedef Eigen::Matrix<double, n, n> A_t;
56 typedef Eigen::Matrix<double, n, m> B_t;
57 typedef Eigen::Matrix<double, p, n> H_t;
58 typedef Eigen::Matrix<double, n, p> K_t;
66 struct inverse_exception : public std::exception
67 {
73 const char* what() const throw()
74 {
75 return "LKF: could not compute matrix inversion!!! Fix your covariances (the measurement's is probably too low...)";
76 }
77 };
78 //}
79
80 public:
87 LKF(){};
88
96 LKF(const A_t& A, const B_t& B, const H_t& H) : A(A), B(B), H(H){};
97
98 /* correct() method //{ */
111 virtual statecov_t correct(const statecov_t& sc, const z_t& z, const R_t& R) const override
112 {
113 /* return correct_optimized(sc, z, R, H); */
114 return correction_impl(sc, z, R, H);
115 };
116 //}
117
118 /* predict() method //{ */
138 virtual statecov_t predict(const statecov_t& sc, const u_t& u, const Q_t& Q, [[maybe_unused]] double dt) const override
139 {
140 statecov_t ret;
141 ret.x = state_predict(A, sc.x, B, u);
142 ret.P = covariance_predict(A, sc.P, Q, dt);
143 return ret;
144 };
145 //}
146
147 public:
152 protected:
153 /* covariance_predict() method //{ */
154 static P_t covariance_predict(const A_t& A, const P_t& P, const Q_t& Q, const double dt)
155 {
156 return A * P * A.transpose() + dt * Q;
157 }
158 //}
159
160 /* state_predict() method //{ */
161 template <int check = n_inputs>
162 static inline typename std::enable_if<check == 0, x_t>::type state_predict(const A_t& A, const x_t& x, [[maybe_unused]] const B_t& B,
163 [[maybe_unused]] const u_t& u)
164 {
165 return A * x;
166 }
167
168 template <int check = n_inputs>
169 static inline typename std::enable_if<check != 0, x_t>::type state_predict(const A_t& A, const x_t& x, const B_t& B, const u_t& u)
170 {
171 return A * x + B * u;
172 }
173 //}
174
175 protected:
176 /* invert_W() method //{ */
177 static R_t invert_W(R_t W)
178 {
179 Eigen::ColPivHouseholderQR<R_t> qr(W);
180 if (!qr.isInvertible())
181 {
182 // add some stuff to the tmp matrix diagonal to make it invertible
183 R_t ident = R_t::Identity(W.rows(), W.cols());
184 W += 1e-9 * ident;
185 qr.compute(W);
186 if (!qr.isInvertible())
187 {
188 // never managed to make this happen except for explicitly putting NaNs in the input
189 throw inverse_exception();
190 }
191 }
192 const R_t W_inv = qr.inverse();
193 return W_inv;
194 }
195 //}
196
197 /* computeKalmanGain() method //{ */
198 virtual K_t computeKalmanGain(const statecov_t& sc, [[maybe_unused]] const z_t& z, const R_t& R, const H_t& H) const
199 {
200 // calculation of the kalman gain K
201 const R_t W = H * sc.P * H.transpose() + R;
202 const R_t W_inv = invert_W(W);
203 const K_t K = sc.P * H.transpose() * W_inv;
204 return K;
205 }
206 //}
207
208 /* correction_impl() method //{ */
209 template <int check = n>
210 typename std::enable_if<check >= 0, statecov_t>::type correction_impl(const statecov_t& sc, const z_t& z, const R_t& R, const H_t& H) const
211 {
212 // the correction phase
213 statecov_t ret;
214 const K_t K = computeKalmanGain(sc, z, R, H);
215 ret.x = sc.x + K * (z - (H * sc.x));
216 ret.P = (P_t::Identity() - (K * H)) * sc.P;
217 return ret;
218 }
219
220 template <int check = n>
221 typename std::enable_if < check<0, statecov_t>::type correction_impl(const statecov_t& sc, const z_t& z, const R_t& R, const H_t& H) const
222 {
223 // the correction phase
224 statecov_t ret;
225 const K_t K = computeKalmanGain(sc, z, R, H);
226 ret.x = sc.x + K * (z - (H * sc.x));
227 ret.P = (P_t::Identity(sc.P.rows(), sc.P.cols()) - (K * H)) * sc.P;
228 return ret;
229 }
230 //}
231
232 // NOT USED METHODS
233 /* correction_optimized() method //{ */
234 // No notable performance gain was observed for the matrix sizes we use, so this is not used.
235 static statecov_t correction_optimized(const statecov_t& sc, const z_t& z, const R_t& R, const H_t& H)
236 {
237 statecov_t ret = sc;
238 const H_t B(H * sc.P.transpose());
239 const K_t K((B * H.transpose() + R).ldlt().solve(B).transpose());
240 ret.x.noalias() += K * (z - H * sc.x);
241 ret.P.noalias() -= K * H * sc.P;
242 return ret;
243 }
244
245 /* static statecov_t correction_optimized(const statecov_t& sc, const z_t& z, const R_t& R, const H_t& H) */
246 /* { */
247 /* statecov_t ret; */
248 /* const H_t B = H*sc.P.transpose(); */
249 /* const K_t K = (B*H.transpose() + R).partialPivLu().solve(B).transpose(); */
250 /* ret.x = sc.x + K*(z - H*sc.x); */
251 /* ret.P = sc.P - K*H*sc.P; */
252 /* return ret; */
253 /* } */
254 //}
255 };
256 //}
257
258 /* class dtMatrixLKF //{ */
259 template <int n_states, int n_inputs, int n_measurements>
260 class varstepLKF : public LKF<n_states, n_inputs, n_measurements>
261 {
262 public:
263 /* LKF definitions (typedefs, constants etc) //{ */
264 static const int n = n_states;
265 static const int m = n_inputs;
266 static const int p = n_measurements;
267 using Base_class = LKF<n, m, p>;
268
269 using x_t = typename Base_class::x_t;
270 using u_t = typename Base_class::u_t;
271 using z_t = typename Base_class::z_t;
272 using P_t = typename Base_class::P_t;
273 using R_t = typename Base_class::R_t;
274 using statecov_t = typename Base_class::statecov_t;
275 using A_t = typename Base_class::A_t; // measurement mapping p*n
276 using B_t = typename Base_class::B_t; // process covariance n*n
277 using H_t = typename Base_class::H_t; // measurement mapping p*n
278 using Q_t = typename Base_class::Q_t; // process covariance n*n
279
280 using generateA_t = std::function<A_t(double)>;
281 using generateB_t = std::function<B_t(double)>;
282 //}
283
284 public:
292 varstepLKF(const generateA_t& generateA, const generateB_t& generateB, const H_t& H) : m_generateA(generateA), m_generateB(generateB)
293 {
295 };
296
297 /* predict() method //{ */
315 virtual statecov_t predict(const statecov_t& sc, const u_t& u, const Q_t& Q, double dt) const override
316 {
317 statecov_t ret;
318 A_t A = m_generateA(dt);
319 B_t B = m_generateB(dt);
320 ret.x = Base_class::state_predict(A, sc.x, B, u);
321 ret.P = Base_class::covariance_predict(A, sc.P, Q, dt);
322 return ret;
323 };
324 //}
325
326 private:
327 generateA_t m_generateA;
328 generateB_t m_generateB;
329 };
330 //}
331
332 /* class LKF_MRS_odom //{ */
333 class LKF_MRS_odom : public LKF<3, 1, 1>
334 {
335 public:
336 /* LKF definitions (typedefs, constants etc) //{ */
337 static const int n = 3;
338 static const int m = 1;
339 static const int p = 1;
340 using Base_class = LKF<n, m, p>;
341
342 using x_t = typename Base_class::x_t;
343 using u_t = typename Base_class::u_t;
344 using z_t = typename Base_class::z_t;
345 using P_t = typename Base_class::P_t;
346 using R_t = typename Base_class::R_t;
347 using statecov_t = typename Base_class::statecov_t;
348 using A_t = typename Base_class::A_t; // measurement mapping p*n
349 using B_t = typename Base_class::B_t; // process covariance n*n
350 using H_t = typename Base_class::H_t; // measurement mapping p*n
351 using Q_t = typename Base_class::Q_t; // process covariance n*n
352
353 using coeff_A_t = A_t; // matrix of constant coefficients in matrix A
354 typedef Eigen::Matrix<unsigned, n, n> dtexp_A_t; // matrix of dt exponents in matrix A
355 using coeff_B_t = B_t; // matrix of constant coefficients in matrix B
356 typedef Eigen::Matrix<unsigned, n, m> dtexp_B_t; // matrix of dt exponents in matrix B
357 //}
358
359 public:
360 LKF_MRS_odom(const std::vector<H_t>& Hs, const double default_dt = 1);
361 virtual statecov_t predict(const statecov_t& sc, const u_t& u, const Q_t& Q, double dt) const override;
362 /* virtual statecov_t correct(const statecov_t& sc, const z_t& z, const R_t& R, int param = 0) const; */
363
364 public:
365 x_t state_predict_optimized(const x_t& x_prev, const u_t& u, double dt) const;
366 P_t covariance_predict_optimized(const P_t& P, const Q_t& Q, double dt) const;
367
368 private:
369 std::vector<H_t> m_Hs;
370 };
371 //}
372
373} // namespace mrs_lib
374
375#endif // LKFSYSTEMMODELS_H
This abstract class defines common interfaces and types for a generic Kalman filter.
Definition kalman_filter.h:26
Eigen::Matrix< double, p, 1 > z_t
Measurement vector type .
Definition kalman_filter.h:35
Eigen::Matrix< double, p, p > R_t
Measurement noise covariance matrix type .
Definition kalman_filter.h:38
Eigen::Matrix< double, n, 1 > x_t
State vector type .
Definition kalman_filter.h:33
Eigen::Matrix< double, n, n > P_t
State uncertainty covariance matrix type .
Definition kalman_filter.h:37
Eigen::Matrix< double, n, n > Q_t
Process noise covariance matrix type .
Definition kalman_filter.h:39
Eigen::Matrix< double, m, 1 > u_t
Input vector type .
Definition kalman_filter.h:34
Definition lkf.h:334
Implementation of the Linear Kalman filter .
Definition lkf.h:39
A_t A
The system transition matrix .
Definition lkf.h:148
static constexpr int n
Length of the state vector of the system.
Definition lkf.h:42
B_t B
The input to state mapping matrix .
Definition lkf.h:149
Eigen::Matrix< double, n, p > K_t
Kalman gain matrix type .
Definition lkf.h:58
typename Base_class::statecov_t statecov_t
Helper struct for passing around the state and its covariance in one variable.
Definition lkf.h:53
H_t H
The state to measurement mapping matrix .
Definition lkf.h:150
Eigen::Matrix< double, n, m > B_t
Input to state mapping matrix type .
Definition lkf.h:56
typename Base_class::R_t R_t
Measurement noise covariance matrix type .
Definition lkf.h:51
typename Base_class::P_t P_t
State uncertainty covariance matrix type .
Definition lkf.h:50
typename Base_class::Q_t Q_t
Process noise covariance matrix type .
Definition lkf.h:52
Eigen::Matrix< double, p, n > H_t
State to measurement mapping matrix type .
Definition lkf.h:57
static constexpr int p
Length of the measurement vector of the system.
Definition lkf.h:44
virtual statecov_t correct(const statecov_t &sc, const z_t &z, const R_t &R) const override
Applies the correction (update, measurement, data) step of the Kalman filter.
Definition lkf.h:111
LKF(const A_t &A, const B_t &B, const H_t &H)
The main constructor.
Definition lkf.h:96
typename Base_class::u_t u_t
Input vector type .
Definition lkf.h:48
Eigen::Matrix< double, n, n > A_t
System transition matrix type .
Definition lkf.h:55
static constexpr int m
Length of the input vector of the system.
Definition lkf.h:43
virtual statecov_t predict(const statecov_t &sc, const u_t &u, const Q_t &Q, double dt) const override
Applies the prediction (time) step of the Kalman filter.
Definition lkf.h:138
typename Base_class::x_t x_t
State vector type .
Definition lkf.h:47
LKF()
Convenience default constructor.
Definition lkf.h:87
typename Base_class::z_t z_t
Measurement vector type .
Definition lkf.h:49
Definition lkf.h:261
virtual statecov_t predict(const statecov_t &sc, const u_t &u, const Q_t &Q, double dt) const override
Applies the prediction (time) step of the Kalman filter.
Definition lkf.h:315
varstepLKF(const generateA_t &generateA, const generateB_t &generateB, const H_t &H)
The main constructor.
Definition lkf.h:292
Defines KalmanFilter - an abstract class, defining common interfaces and types for a generic Kalman f...
All mrs_lib functions, classes, variables and definitions are contained in this namespace.
Definition attitude_converter.h:24
Helper struct for passing around the state and its covariance in one variable.
Definition kalman_filter.h:47
This exception is thrown when taking the inverse of a matrix fails.
Definition lkf.h:67
const char * what() const
Returns the error message, describing what caused the exception.
Definition lkf.h:73