Go to the documentation of this file.
16 #ifndef SURGSIM_MATH_KALMANFILTER_INL_H
17 #define SURGSIM_MATH_KALMANFILTER_INL_H
24 template <
size_t M,
size_t N>
26 m_stateTransition(
Eigen::
Matrix<double, M, M,
Eigen::RowMajor>::Constant(std::numeric_limits<double>::quiet_NaN())),
28 std::numeric_limits<double>::quiet_NaN())),
29 m_processNoiseCovariance(
Eigen::
Matrix<double, M, M,
Eigen::RowMajor>::Constant(
30 std::numeric_limits<double>::quiet_NaN())),
31 m_measurementNoiseCovariance(
Eigen::
Matrix<double, N, N,
Eigen::RowMajor>::Constant(
32 std::numeric_limits<double>::quiet_NaN())),
33 m_state(
Eigen::
Matrix<double, M, 1>::Constant(std::numeric_limits<double>::quiet_NaN())),
34 m_stateCovariance(
Eigen::
Matrix<double, M, M,
Eigen::RowMajor>::Constant(std::numeric_limits<double>::quiet_NaN()))
38 template <
size_t M,
size_t N>
43 template <
size_t M,
size_t N>
49 template <
size_t M,
size_t N>
52 m_stateCovariance = p;
55 template <
size_t M,
size_t N>
58 m_stateTransition = a;
61 template <
size_t M,
size_t N>
64 m_observationMatrix = h;
67 template <
size_t M,
size_t N>
70 m_processNoiseCovariance = q;
73 template <
size_t M,
size_t N>
76 m_measurementNoiseCovariance = r;
79 template <
size_t M,
size_t N>
86 template <
size_t M,
size_t N>
87 const Eigen::Matrix<double, M, 1>&
91 updateMeasurement(measurement);
95 template <
size_t M,
size_t N>
101 template <
size_t M,
size_t N>
104 m_state = m_stateTransition * m_state;
105 m_stateCovariance = m_stateTransition * m_stateCovariance * m_stateTransition.transpose() +
106 m_processNoiseCovariance;
109 template <
size_t M,
size_t N>
112 const Matrix gain = m_stateCovariance * m_observationMatrix.transpose() *
113 (m_observationMatrix * m_stateCovariance * m_observationMatrix.transpose() +
114 m_measurementNoiseCovariance).inverse();
115 m_state += gain * (measurement - m_observationMatrix * m_state);
116 m_stateCovariance -= gain * m_observationMatrix * m_stateCovariance;
122 #endif // SURGSIM_MATH_KALMANFILTER_INL_H
Definition: MathUtilities.h:95
const Eigen::Matrix< double, M, 1 > & getState() const
Get the current state.
Definition: KalmanFilter-inl.h:96
const Eigen::Matrix< double, M, 1 > & update()
Advance one step without a measurement.
Definition: KalmanFilter-inl.h:80
void setMeasurementNoiseCovariance(const Eigen::Ref< const Eigen::Matrix< double, N, N >> &r)
Set the measurement noise covariance, size n x n.
Definition: KalmanFilter-inl.h:74
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.
Definition: KalmanFilter-inl.h:50
Definition: CompoundShapeToGraphics.cpp:30
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.
Definition: KalmanFilter-inl.h:62
void updatePrediction()
Use the current estimated state, x(t), and matrices to predict the new state, x(t+1),...
Definition: KalmanFilter-inl.h:102
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,...
Definition: KalmanFilter-inl.h:110
KalmanFilter()
Constructor.
Definition: KalmanFilter-inl.h:25
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.
Definition: KalmanFilter-inl.h:56
void setInitialState(const Eigen::Ref< const Eigen::Matrix< double, M, 1 >> &x)
Set the initial state vector, x(0), length m.
Definition: KalmanFilter-inl.h:44
void setProcessNoiseCovariance(const Eigen::Ref< const Eigen::Matrix< double, M, M >> &q)
Set the process noise covariance, size m x m.
Definition: KalmanFilter-inl.h:68
virtual ~KalmanFilter()
Destructor.
Definition: KalmanFilter-inl.h:39
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > Matrix
A dynamic size matrix.
Definition: Matrix.h:65