Linear Kalman Filter implementation. See: An Introduction to the Kalman Filter, TR 95-041 by Greg Welch1 and Gary Bishop https://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf. More...
#include <kalman_filter.h>
Public Types | |
| typedef KalmanFilterConfig< MotionModelType, ObservationModelType > | config_type |
| typedef DynMat< real_t > | matrix_type |
| typedef MotionModelType | motion_model_type |
| typedef ObservationModelType | observation_model_type |
| typedef config_type::motion_model_type::state_type | state_type |
| typedef std::map< std::string, std::any > | input_type |
Public Member Functions | |
| KalmanFilter (const KalmanFilterConfig< MotionModelType, ObservationModelType > &config) | |
| KalmanFilter Constructor. | |
| state_type & | estimate (const input_type &input) |
| Estimate the state. This function simply wraps the predict and update steps described by the functions below. | |
| void | predict (const input_type &input) |
| Predicts the state vector x and the process covariance matrix P using the given input control u accroding to the following equations. | |
| void | update (const input_type &input) |
| Updates the gain matrix \(K\), the state vector \(x\) and covariance matrix P using the given measurement z_k according to the following equations. | |
| void | set_motion_model (motion_model_type &motion_model) |
| Set the motion model. | |
| void | set_observation_model (const observation_model_type &observation_model) |
| Set the observation model. | |
| KalmanFilter & | with_matrix (const std::string &name, const matrix_type &mat) |
| Set the matrix and return *this. | |
| void | set_matrix (const std::string &name, const matrix_type &mat) |
| Set the matrix used by the filter. | |
| bool | has_matrix (const std::string &name) const |
| Returns true if the matrix with the given name exists. | |
| const state_type & | get_state () const |
| Returns the state. | |
| state_type & | get_state () |
| Returns the state. | |
| real_t | get (const std::string &name) const |
| Returns the state property with the given name. | |
| const matrix_type & | operator[] (const std::string &name) const |
| Returns the name-th matrix. | |
| matrix_type & | operator[] (const std::string &name) |
| Returns the name-th matrix. | |
Detailed Description
class bitrl::estimation::KalmanFilter< MotionModelType, ObservationModelType >
Linear Kalman Filter implementation. See: An Introduction to the Kalman Filter, TR 95-041 by Greg Welch1 and Gary Bishop https://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf.
The algorithm is implemented as follows:
prediction step:
\[\hat{\mathbf{x}}_{k} = F_k \mathbf{x}_{k-1} + B_k \mathbf{u}_k + \mathbf{w}_k\]
\[\hat{P}_{k} = F_{k-1} P_{k-1} F_{k-1}^T + Q_{k-1}\]
update step:
\[K_k = \hat{P}_{k} H_{k}^T * (H_k \hat{P}_{k} * H_{k}^T + R_k )^{-1}\]
\[\mathbf{x}_k = \hat{\mathbf{x}}_{k} + K_k (z_k - h( \hat{x}_{k}, 0))\]
\[P_k = (I - K_k H_k) \hat{P}_{k}\]
where \(w_k\) and \(v_k\) represent process and measurement noise respectively. They are assumed independent and normally distributed:
\[p(w) \sim N(0,Q)\]
\[p(v) \sim N(0,R)\]
The gain matrix K says how much the predictions should be corrected The following matrices dimensions are assumed:
state vector: x n x 1 control vector: u l x 1 meas. vector: y m x 1
\[ \mathbf{F} n \times n \]
\[ \mathfb{P} n \times n \]
\[ \mathbf{B} n \times l \]
\[ \mathbf{H} m \times n \]
\[ \mathbf{K} n \times m \]
\[ \mathbf{Q} n \times n \]
\[ \mathbf{R} m \times m \]
Member Typedef Documentation
◆ config_type
| typedef KalmanFilterConfig<MotionModelType, ObservationModelType> bitrl::estimation::KalmanFilter< MotionModelType, ObservationModelType >::config_type |
◆ input_type
| typedef std::map<std::string, std::any> bitrl::estimation::KalmanFilter< MotionModelType, ObservationModelType >::input_type |
◆ matrix_type
| typedef DynMat<real_t> bitrl::estimation::KalmanFilter< MotionModelType, ObservationModelType >::matrix_type |
◆ motion_model_type
| typedef MotionModelType bitrl::estimation::KalmanFilter< MotionModelType, ObservationModelType >::motion_model_type |
◆ observation_model_type
| typedef ObservationModelType bitrl::estimation::KalmanFilter< MotionModelType, ObservationModelType >::observation_model_type |
◆ state_type
| typedef config_type::motion_model_type::state_type bitrl::estimation::KalmanFilter< MotionModelType, ObservationModelType >::state_type |
Constructor & Destructor Documentation
◆ KalmanFilter()
| bitrl::estimation::KalmanFilter< MotionModelType, ObservationModelType >::KalmanFilter | ( | const KalmanFilterConfig< MotionModelType, ObservationModelType > & | config | ) |
KalmanFilter Constructor.
Member Function Documentation
◆ estimate()
| KalmanFilter< MotionModelType, ObservationModelType >::state_type & bitrl::estimation::KalmanFilter< MotionModelType, ObservationModelType >::estimate | ( | const input_type & | input | ) |
Estimate the state. This function simply wraps the predict and update steps described by the functions below.
◆ get()
|
inline |
Returns the state property with the given name.
◆ get_state() [1/2]
|
inline |
Returns the state.
◆ get_state() [2/2]
|
inline |
Returns the state.
◆ has_matrix()
| bool bitrl::estimation::KalmanFilter< MotionModelType, ObservationModelType >::has_matrix | ( | const std::string & | name | ) | const |
Returns true if the matrix with the given name exists.
◆ operator[]() [1/2]
| DynMat< real_t > & bitrl::estimation::KalmanFilter< MotionModelType, ObservationModelType >::operator[] | ( | const std::string & | name | ) |
Returns the name-th matrix.
◆ operator[]() [2/2]
| const DynMat< real_t > & bitrl::estimation::KalmanFilter< MotionModelType, ObservationModelType >::operator[] | ( | const std::string & | name | ) | const |
Returns the name-th matrix.
◆ predict()
| void bitrl::estimation::KalmanFilter< MotionModelType, ObservationModelType >::predict | ( | const input_type & | input | ) |
Predicts the state vector x and the process covariance matrix P using the given input control u accroding to the following equations.
\[\hat{x}_{k = F_k* x_{k-1} + B_k * u_k + w_k\]
\[\hat{P}_{k} = F_{k-1} * P_{k-1} * F_{k-1}^T + Q_{k-1}\]
where \(x_{k-1}\) is the state at the previous step, \(u_{k}\) is the control signal and w_k is the error associated with the control signal. In input argument passed to the function is meant to model in a tuple all the arguments needed. F, is the dynamics matrix and Q is the covariance matrix associate with the control signal
The control input argument should supply both \(u_k\) and \(w_k\) vectors
◆ set_matrix()
| void bitrl::estimation::KalmanFilter< MotionModelType, ObservationModelType >::set_matrix | ( | const std::string & | name, |
| const matrix_type & | mat | ||
| ) |
Set the matrix used by the filter.
◆ set_motion_model()
|
inline |
Set the motion model.
◆ set_observation_model()
|
inline |
Set the observation model.
◆ update()
| void bitrl::estimation::KalmanFilter< MotionModelType, ObservationModelType >::update | ( | const input_type & | input | ) |
Updates the gain matrix \(K\), the state vector \(x\) and covariance matrix P using the given measurement z_k according to the following equations.
K_k = \hat{P}_{k} * H_{k}^T * (H_k * \hat{P}_{k} * H_{k}^T + R_k )^{-1} x_k = \hat{x}_{k} + K_k * (z_k - H * \hat{x}_{k} P_k = (I - K_k * H_k) * \hat{P}_{k}
◆ with_matrix()
| KalmanFilter< MotionModelType, ObservationModelType > & bitrl::estimation::KalmanFilter< MotionModelType, ObservationModelType >::with_matrix | ( | const std::string & | name, |
| const matrix_type & | mat | ||
| ) |
Set the matrix and return *this.
The documentation for this class was generated from the following file:
- libs/bitrl/src/bitrl/estimation/kalman_filter.h
