Public Member Functions | Private Member Functions | Private Attributes | List of all members
SurgSim::Math::KalmanFilter< M, N > Class Template Reference

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...
 

Detailed Description

template<size_t M, size_t N>
class SurgSim::Math::KalmanFilter< M, N >

Implements a linear Kalman filter, a recursive estimator.

Does not support control inputs.

Template Parameters
MThe length of the state vector.
NThe length of the measurement vector.

Constructor & Destructor Documentation

◆ KalmanFilter()

template<size_t M, size_t N>
SurgSim::Math::KalmanFilter< M, N >::KalmanFilter ( )

Constructor.

◆ ~KalmanFilter()

template<size_t M, size_t N>
SurgSim::Math::KalmanFilter< M, N >::~KalmanFilter ( )
virtual

Destructor.

Member Function Documentation

◆ getState()

template<size_t M, size_t N>
const Eigen::Matrix< double, M, 1 > & SurgSim::Math::KalmanFilter< M, N >::getState ( ) const

Get the current state.

Does not advance the state.

Returns
The estimate for the current state, x(t), length m.

◆ setInitialState()

template<size_t M, size_t N>
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.

Parameters
xThe initial state.

◆ setInitialStateCovariance()

template<size_t M, size_t N>
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.

Parameters
pThe initial covariance.

◆ setMeasurementNoiseCovariance()

template<size_t M, size_t N>
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.

Parameters
rThe measurement noise covariance.

◆ setObservationMatrix()

template<size_t M, size_t N>
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.

Parameters
hThe observation matrix.

◆ setProcessNoiseCovariance()

template<size_t M, size_t N>
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.

Parameters
qThe process noise covariance.

◆ setStateTransition()

template<size_t M, size_t N>
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.

Parameters
aThe state transition matrix.

◆ update() [1/2]

template<size_t M, size_t N>
const Eigen::Matrix< double, M, 1 > & SurgSim::Math::KalmanFilter< M, N >::update ( )

Advance one step without a measurement.

Returns
The estimate for the new state, x(t+1), length m.

◆ update() [2/2]

template<size_t M, size_t N>
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.

Parameters
measurementThe measurement, z(t), length n.
Returns
The estimate for the new state, x(t+1), length m.

◆ updateMeasurement()

template<size_t M, size_t N>
void SurgSim::Math::KalmanFilter< M, N >::updateMeasurement ( const Eigen::Ref< const Eigen::Matrix< double, N, 1 >> &  measurement)
private

Correct the current estimated state, x(t), and state covariance, P(t), based on a measurement, z(t).

Parameters
measurementThe measurement, length n.

◆ updatePrediction()

template<size_t M, size_t N>
void SurgSim::Math::KalmanFilter< M, N >::updatePrediction ( )
private

Use the current estimated state, x(t), and matrices to predict the new state, x(t+1), and state covariance, P(t+1).

Member Data Documentation

◆ m_measurementNoiseCovariance

template<size_t M, size_t N>
Eigen::Matrix<double, N, N, Eigen::RowMajor> SurgSim::Math::KalmanFilter< M, N >::m_measurementNoiseCovariance
private

The measurement noise covariance.

◆ m_observationMatrix

template<size_t M, size_t N>
Eigen::Matrix<double, N, M, Eigen::RowMajor> SurgSim::Math::KalmanFilter< M, N >::m_observationMatrix
private

The observation matrix.

◆ m_processNoiseCovariance

template<size_t M, size_t N>
Eigen::Matrix<double, M, M, Eigen::RowMajor> SurgSim::Math::KalmanFilter< M, N >::m_processNoiseCovariance
private

The process noise covariance.

◆ m_state

template<size_t M, size_t N>
Eigen::Matrix<double, M, 1> SurgSim::Math::KalmanFilter< M, N >::m_state
private

The state.

◆ m_stateCovariance

template<size_t M, size_t N>
Eigen::Matrix<double, M, M, Eigen::RowMajor> SurgSim::Math::KalmanFilter< M, N >::m_stateCovariance
private

The covariance of the state.

◆ m_stateTransition

template<size_t M, size_t N>
Eigen::Matrix<double, M, M, Eigen::RowMajor> SurgSim::Math::KalmanFilter< M, N >::m_stateTransition
private

The state transition matrix.


The documentation for this class was generated from the following files: