![]() |
mrs_lib
Various reusable classes, functions and utilities for use in MRS projects
|
This abstract class defines common interfaces and types for a generic Kalman filter. More...
#include <kalman_filter.h>
Classes | |
struct | statecov_t |
Helper struct for passing around the state and its covariance in one variable. More... | |
Public Types | |
typedef Eigen::Matrix< double, n, 1 > | x_t |
State vector type ![]() | |
typedef Eigen::Matrix< double, m, 1 > | u_t |
Input vector type ![]() | |
typedef Eigen::Matrix< double, p, 1 > | z_t |
Measurement vector type ![]() | |
typedef Eigen::Matrix< double, n, n > | P_t |
State uncertainty covariance matrix type ![]() | |
typedef Eigen::Matrix< double, p, p > | R_t |
Measurement noise covariance matrix type ![]() | |
typedef Eigen::Matrix< double, n, n > | Q_t |
Process noise covariance matrix type ![]() | |
Public Member Functions | |
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. | |
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. | |
This abstract class defines common interfaces and types for a generic Kalman filter.
n_states | number of states of the system (length of the ![]() |
n_inputs | number of inputs of the system (length of the ![]() |
n_measurements | number of measurements of the system (length of the ![]() |
|
pure virtual |
Applies the correction (update, measurement, data) step of the Kalman filter.
This method applies the correction step to the state and covariance passed in sc
using the measurement z
and measurement noise R
. An optional parameter param
may be used by some implementations, but it is usually ignored. The updated state and covariance after the correction step is returned.
sc | The state and covariance to which the correction step is to be applied. |
z | The measurement vector to be used for correction. |
R | The measurement noise covariance matrix to be used for correction. |
Implemented in mrs_lib::LKF< n_states, n_inputs, n_measurements >, mrs_lib::LKF< 3, 1, 1 >, mrs_lib::LKF< n_states, n_inputs, -1 >, and mrs_lib::UKF< n_states, n_inputs, n_measurements >.
|
pure virtual |
Applies the prediction (time) step of the Kalman filter.
This method applies the prediction step to the state and covariance passed in sc
using the input u
and process noise Q
. The state and covariance are updated by dt
into the future, if applicable to the implementation. An optional parameter param
may be used by some implementations, but it is usually ignored. The updated state and covariance after the prediction step is returned.
sc | The state and covariance to which the prediction step is to be applied. |
u | The input vector to be used for prediction. |
Q | The process noise covariance matrix to be used for prediction. |
dt | The time step for the prediction update (the state and covariance will be predicted by dt into the future). |
Implemented in mrs_lib::LKF< n_states, n_inputs, n_measurements >, mrs_lib::LKF< 3, 1, 1 >, mrs_lib::LKF< n_states, n_inputs, -1 >, and mrs_lib::UKF< n_states, n_inputs, n_measurements >.