mrs_lib
Various reusable classes, functions and utilities for use in MRS projects
Classes | Public Types | Public Member Functions | Public Attributes | Static Public Attributes | Protected Member Functions | Static Protected Member Functions | List of all members
mrs_lib::LKF< n_states, n_inputs, n_measurements > Class Template Reference

Implementation of the Linear Kalman filter [3]. More...

#include <lkf.h>

+ Inheritance diagram for mrs_lib::LKF< n_states, n_inputs, n_measurements >:
+ Collaboration diagram for mrs_lib::LKF< n_states, n_inputs, n_measurements >:

Classes

struct  inverse_exception
 This exception is thrown when taking the inverse of a matrix fails. More...
 

Public Types

using Base_class = KalmanFilter< n, m, p >
 Base class of this class.
 
using x_t = typename Base_class::x_t
 State vector type $n \times 1$.
 
using u_t = typename Base_class::u_t
 Input vector type $m \times 1$.
 
using z_t = typename Base_class::z_t
 Measurement vector type $p \times 1$.
 
using P_t = typename Base_class::P_t
 State uncertainty covariance matrix type $n \times n$.
 
using R_t = typename Base_class::R_t
 Measurement noise covariance matrix type $p \times p$.
 
using Q_t = typename Base_class::Q_t
 Process noise covariance matrix type $n \times n$.
 
using statecov_t = typename Base_class::statecov_t
 Helper struct for passing around the state and its covariance in one variable.
 
typedef Eigen::Matrix< double, n, n > A_t
 System transition matrix type $n \times n$.
 
typedef Eigen::Matrix< double, n, m > B_t
 Input to state mapping matrix type $n \times m$.
 
typedef Eigen::Matrix< double, p, n > H_t
 State to measurement mapping matrix type $p \times n$.
 
typedef Eigen::Matrix< double, n, p > K_t
 Kalman gain matrix type $n \times p$.
 
- Public Types inherited from mrs_lib::KalmanFilter< n_states, n_inputs, n_measurements >
typedef Eigen::Matrix< double, n, 1 > x_t
 State vector type $n \times 1$.
 
typedef Eigen::Matrix< double, m, 1 > u_t
 Input vector type $m \times 1$.
 
typedef Eigen::Matrix< double, p, 1 > z_t
 Measurement vector type $p \times 1$.
 
typedef Eigen::Matrix< double, n, n > P_t
 State uncertainty covariance matrix type $n \times n$.
 
typedef Eigen::Matrix< double, p, p > R_t
 Measurement noise covariance matrix type $p \times p$.
 
typedef Eigen::Matrix< double, n, n > Q_t
 Process noise covariance matrix type $n \times n$.
 

Public Member Functions

 LKF ()
 Convenience default constructor. More...
 
 LKF (const A_t &A, const B_t &B, const H_t &H)
 The main constructor. More...
 
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. More...
 
virtual statecov_t predict (const statecov_t &sc, const u_t &u, const Q_t &Q, [[maybe_unused]] double dt) const override
 Applies the prediction (time) step of the Kalman filter. More...
 
- Public Member Functions inherited from mrs_lib::KalmanFilter< n_states, n_inputs, n_measurements >
virtual statecov_t correct (const statecov_t &sc, const z_t &z, const R_t &R) const =0
 Applies the correction (update, measurement, data) step of the Kalman filter. More...
 
virtual statecov_t predict (const statecov_t &sc, const u_t &u, const Q_t &Q, double dt) const =0
 Applies the prediction (time) step of the Kalman filter. More...
 

Public Attributes

A_t A
 The system transition matrix $n \times n$.
 
B_t B
 The input to state mapping matrix $n \times m$.
 
H_t H
 The state to measurement mapping matrix $p \times n$.
 

Static Public Attributes

static constexpr int n = n_states
 Length of the state vector of the system.
 
static constexpr int m = n_inputs
 Length of the input vector of the system.
 
static constexpr int p = n_measurements
 Length of the measurement vector of the system.
 
- Static Public Attributes inherited from mrs_lib::KalmanFilter< n_states, n_inputs, n_measurements >
static const int n = n_states
 Length of the state vector of the system.
 
static const int m = n_inputs
 Length of the input vector of the system.
 
static const int p = n_measurements
 Length of the measurement vector of the system.
 

Protected Member Functions

virtual K_t computeKalmanGain (const statecov_t &sc, [[maybe_unused]] const z_t &z, const R_t &R, const H_t &H) const
 
statecov_t ::type correction_impl (const statecov_t &sc, const z_t &z, const R_t &R, const H_t &H) const
 

Static Protected Member Functions

static P_t covariance_predict (const A_t &A, const P_t &P, const Q_t &Q, const double dt)
 
template<int check = n_inputs>
static std::enable_if< check==0, x_t >::type state_predict (const A_t &A, const x_t &x, [[maybe_unused]] const B_t &B, [[maybe_unused]] const u_t &u)
 
template<int check = n_inputs>
static 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)
 
static R_t invert_W (R_t W)
 

Detailed Description

template<int n_states, int n_inputs, int n_measurements>
class mrs_lib::LKF< n_states, n_inputs, n_measurements >

Implementation of the Linear Kalman filter [3].

The Linear Kalman filter (abbreviated LKF, [3]) may be used for state filtration or estimation of linear stochastic discrete systems. It assumes that noise variables are sampled from multivariate gaussian distributions and takes into account apriori known parameters of these distributions (namely zero means and covariance matrices, which have to be specified by the user and are tunable parameters).

The LKF C++ class itself is templated. This has its advantages and disadvantages. Main disadvantage is that it may be harder to use if you're not familiar with C++ templates, which, admittedly, can get somewhat messy, espetially during compilation. Another disadvantage is that if used unwisely, the compilation times can get much higher when using templates. The main advantage is compile-time checking (if it compiles, then it has a lower chance of crashing at runtime) and enabling more effective optimalizations during compilation. Also in case of Eigen, the code is arguably more readable when you use aliases to the specific Matrix instances instead of having Eigen::MatrixXd and Eigen::VectorXd everywhere.

Template Parameters
n_statesnumber of states of the system (length of the $ \mathbf{x} $ vector).
n_inputsnumber of inputs of the system (length of the $ \mathbf{u} $ vector).
n_measurementsnumber of measurements of the system (length of the $ \mathbf{z} $ vector).
Examples
lkf/example.cpp.

Constructor & Destructor Documentation

◆ LKF() [1/2]

template<int n_states, int n_inputs, int n_measurements>
mrs_lib::LKF< n_states, n_inputs, n_measurements >::LKF ( )
inline

Convenience default constructor.

This constructor should not be used if applicable. If used, the main constructor has to be called afterwards, before using this class, otherwise the LKF object is invalid (not initialized).

◆ LKF() [2/2]

template<int n_states, int n_inputs, int n_measurements>
mrs_lib::LKF< n_states, n_inputs, n_measurements >::LKF ( const A_t &  A,
const B_t &  B,
const H_t &  H 
)
inline

The main constructor.

Parameters
AThe state transition matrix.
BThe input to state mapping matrix.
HThe state to measurement mapping matrix.

Member Function Documentation

◆ correct()

template<int n_states, int n_inputs, int n_measurements>
virtual statecov_t mrs_lib::LKF< n_states, n_inputs, n_measurements >::correct ( const statecov_t &  sc,
const z_t &  z,
const R_t &  R 
) const
inlineoverridevirtual

Applies the correction (update, measurement, data) step of the Kalman filter.

This method applies the linear Kalman filter correction step to the state and covariance passed in sc using the measurement z and measurement noise R. The updated state and covariance after the correction step is returned.

Parameters
scThe state and covariance to which the correction step is to be applied.
zThe measurement vector to be used for correction.
RThe measurement noise covariance matrix to be used for correction.
Returns
The state and covariance after the correction update.

◆ predict()

template<int n_states, int n_inputs, int n_measurements>
virtual statecov_t mrs_lib::LKF< n_states, n_inputs, n_measurements >::predict ( const statecov_t &  sc,
const u_t &  u,
const Q_t &  Q,
[[maybe_unused] ] double  dt 
) const
inlineoverridevirtual

Applies the prediction (time) step of the Kalman filter.

This method applies the linear Kalman filter prediction step to the state and covariance passed in sc using the input u and process noise Q. The process noise covariance Q is scaled by the dt parameter. The updated state and covariance after the prediction step is returned.

Parameters
scThe state and covariance to which the prediction step is to be applied.
uThe input vector to be used for prediction.
QThe process noise covariance matrix to be used for prediction.
dtUsed to scale the process noise covariance Q.
Returns
The state and covariance after the prediction step.
Note
Note that the dt parameter is only used to scale the process noise covariance Q it does not change the system matrices A or B (because there is no unambiguous way to do this)! If you have a changing time step duration and a dynamic system, you have to change the A and B matrices manually.
The documentation for this class was generated from the following file: