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

  M | The length of the state vector.
  N | The 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

  x | The 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

  p | The 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

  r | The 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

  h | The 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

  q | The 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

  a | The 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

  measurement | The 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

  measurement | The 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:

- SurgSim/Math/KalmanFilter.h
- SurgSim/Math/KalmanFilter-inl.h