6#ifndef LKFSYSTEMMODELS_H
7#define LKFSYSTEMMODELS_H
37 template <
int n_states,
int n_inputs,
int n_measurements>
42 static constexpr int n = n_states;
43 static constexpr int m = n_inputs;
44 static constexpr int p = n_measurements;
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;
73 const char*
what()
const throw()
75 return "LKF: could not compute matrix inversion!!! Fix your covariances (the measurement's is probably too low...)";
114 return correction_impl(sc, z, R,
H);
141 ret.x = state_predict(
A, sc.x,
B, u);
142 ret.P = covariance_predict(
A, sc.P, Q, dt);
154 static P_t covariance_predict(
const A_t&
A,
const P_t& P,
const Q_t& Q,
const double dt)
156 return A * P *
A.transpose() + dt * Q;
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)
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)
171 return A * x +
B * u;
177 static R_t invert_W(R_t W)
179 Eigen::ColPivHouseholderQR<R_t> qr(W);
180 if (!qr.isInvertible())
183 R_t ident = R_t::Identity(W.rows(), W.cols());
186 if (!qr.isInvertible())
189 throw inverse_exception();
192 const R_t W_inv = qr.inverse();
198 virtual K_t computeKalmanGain(
const statecov_t& sc, [[maybe_unused]]
const z_t& z,
const R_t& R,
const H_t&
H)
const
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;
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
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;
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
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;
235 static statecov_t correction_optimized(
const statecov_t& sc,
const z_t& z,
const R_t& R,
const H_t&
H)
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;
259 template <
int n_states,
int n_inputs,
int n_measurements>
264 static const int n = n_states;
265 static const int m = n_inputs;
266 static const int p = n_measurements;
280 using generateA_t = std::function<A_t(
double)>;
281 using generateB_t = std::function<B_t(
double)>;
292 varstepLKF(
const generateA_t& generateA,
const generateB_t& generateB,
const H_t&
H) : m_generateA(generateA), m_generateB(generateB)
315 virtual statecov_t
predict(
const statecov_t& sc,
const u_t& u,
const Q_t& Q,
double dt)
const override
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);
327 generateA_t m_generateA;
328 generateB_t m_generateB;
337 static const int n = 3;
338 static const int m = 1;
339 static const int p = 1;
353 using coeff_A_t = A_t;
354 typedef Eigen::Matrix<unsigned, n, n> dtexp_A_t;
355 using coeff_B_t = B_t;
356 typedef Eigen::Matrix<unsigned, n, m> dtexp_B_t;
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;
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;
369 std::vector<H_t> m_Hs;
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
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
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