![]() |
Implements a linear Kalman filter, a recursive estimator. More...
#include <SurgSim/Math/KalmanFilter.h>
Public Member Functions | |
KalmanFilter () | |
Constructor. More... | |
virtual | ~KalmanFilter () |
Destructor. More... | |
void | setInitialState (const Eigen::Ref< const Eigen::Matrix< double, M, 1 >> &x) |
Set the initial state vector, x(0), length m. More... | |
void | setInitialStateCovariance (const Eigen::Ref< const Eigen::Matrix< double, M, M >> &p) |
Set the initial covariance of the state, P(0), size m x m. More... | |
void | setStateTransition (const Eigen::Ref< const Eigen::Matrix< double, M, M >> &a) |
Set the state transition, A, such that x(t+1) = A.x(t), size m x m. More... | |
void | setObservationMatrix (const Eigen::Ref< const Eigen::Matrix< double, N, M >> &h) |
Set the observation matrix, H, such that z(t) = H.x(t), size n x m. More... | |
void | setProcessNoiseCovariance (const Eigen::Ref< const Eigen::Matrix< double, M, M >> &q) |
Set the process noise covariance, size m x m. More... | |
void | setMeasurementNoiseCovariance (const Eigen::Ref< const Eigen::Matrix< double, N, N >> &r) |
Set the measurement noise covariance, size n x n. More... | |
const Eigen::Matrix< double, M, 1 > & | update () |
Advance one step without a measurement. More... | |
const Eigen::Matrix< double, M, 1 > & | update (const Eigen::Ref< const Eigen::Matrix< double, N, 1 >> &measurement) |
Advance one step with measurement. More... | |
const Eigen::Matrix< double, M, 1 > & | getState () const |
Get the current state. More... | |
Private Member Functions | |
void | updatePrediction () |
Use the current estimated state, x(t), and matrices to predict the new state, x(t+1), and state covariance, P(t+1). More... | |
void | updateMeasurement (const Eigen::Ref< const Eigen::Matrix< double, N, 1 >> &measurement) |
Correct the current estimated state, x(t), and state covariance, P(t), based on a measurement, z(t). More... | |
Private Attributes | |
Eigen::Matrix< double, M, M, Eigen::RowMajor > | m_stateTransition |
The state transition matrix. More... | |
Eigen::Matrix< double, N, M, Eigen::RowMajor > | m_observationMatrix |
The observation matrix. More... | |
Eigen::Matrix< double, M, M, Eigen::RowMajor > | m_processNoiseCovariance |
The process noise covariance. More... | |
Eigen::Matrix< double, N, N, Eigen::RowMajor > | m_measurementNoiseCovariance |
The measurement noise covariance. More... | |
Eigen::Matrix< double, M, 1 > | m_state |
The state. More... | |
Eigen::Matrix< double, M, M, Eigen::RowMajor > | m_stateCovariance |
The covariance of the state. More... | |
Implements a linear Kalman filter, a recursive estimator.
Does not support control inputs.
M | The length of the state vector. |
N | The length of the measurement vector. |
SurgSim::Math::KalmanFilter< M, N >::KalmanFilter |
Constructor.
|
virtual |
Destructor.
const Eigen::Matrix< double, M, 1 > & SurgSim::Math::KalmanFilter< M, N >::getState |
Get the current state.
Does not advance the state.
void SurgSim::Math::KalmanFilter< M, N >::setInitialState | ( | const Eigen::Ref< const Eigen::Matrix< double, M, 1 >> & | x | ) |
Set the initial state vector, x(0), length m.
x | The initial state. |
void SurgSim::Math::KalmanFilter< M, N >::setInitialStateCovariance | ( | const Eigen::Ref< const Eigen::Matrix< double, M, M >> & | p | ) |
Set the initial covariance of the state, P(0), size m x m.
p | The initial covariance. |
void SurgSim::Math::KalmanFilter< M, N >::setMeasurementNoiseCovariance | ( | const Eigen::Ref< const Eigen::Matrix< double, N, N >> & | r | ) |
Set the measurement noise covariance, size n x n.
r | The measurement noise covariance. |
void SurgSim::Math::KalmanFilter< M, N >::setObservationMatrix | ( | const Eigen::Ref< const Eigen::Matrix< double, N, M >> & | h | ) |
Set the observation matrix, H, such that z(t) = H.x(t), size n x m.
h | The observation matrix. |
void SurgSim::Math::KalmanFilter< M, N >::setProcessNoiseCovariance | ( | const Eigen::Ref< const Eigen::Matrix< double, M, M >> & | q | ) |
Set the process noise covariance, size m x m.
q | The process noise covariance. |
void SurgSim::Math::KalmanFilter< M, N >::setStateTransition | ( | const Eigen::Ref< const Eigen::Matrix< double, M, M >> & | a | ) |
Set the state transition, A, such that x(t+1) = A.x(t), size m x m.
a | The state transition matrix. |
const Eigen::Matrix< double, M, 1 > & SurgSim::Math::KalmanFilter< M, N >::update |
Advance one step without a measurement.
const Eigen::Matrix< double, M, 1 > & SurgSim::Math::KalmanFilter< M, N >::update | ( | const Eigen::Ref< const Eigen::Matrix< double, N, 1 >> & | measurement | ) |
Advance one step with measurement.
measurement | The measurement, z(t), length n. |
|
private |
Correct the current estimated state, x(t), and state covariance, P(t), based on a measurement, z(t).
measurement | The measurement, length n. |
|
private |
Use the current estimated state, x(t), and matrices to predict the new state, x(t+1), and state covariance, P(t+1).
|
private |
The measurement noise covariance.
|
private |
The observation matrix.
|
private |
The process noise covariance.
|
private |
The state.
|
private |
The covariance of the state.
|
private |
The state transition matrix.