16 #ifndef SURGSIM_MATH_KALMANFILTER_H 17 #define SURGSIM_MATH_KALMANFILTER_H 31 template <
size_t M,
size_t N>
43 void setInitialState(
const Eigen::Ref<
const Eigen::Matrix<double, M, 1>>& x);
67 const Eigen::Matrix<double, M, 1>&
update();
72 const Eigen::Matrix<double, M, 1>&
update(
const Eigen::Ref<
const Eigen::Matrix<double, N, 1>>& measurement);
76 const Eigen::Matrix<double, M, 1>&
getState()
const;
85 void updateMeasurement(
const Eigen::Ref<
const Eigen::Matrix<double, N, 1>>& measurement);
111 #endif // SURGSIM_MATH_KALMANFILTER_H Definition: CompoundShapeToGraphics.cpp:29
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
Eigen::Matrix< double, N, M, Eigen::RowMajor > m_observationMatrix
The observation matrix.
Definition: KalmanFilter.h:91
Implements a linear Kalman filter, a recursive estimator.
Definition: KalmanFilter.h:32
KalmanFilter()
Constructor.
Definition: KalmanFilter-inl.h:25
Eigen::Matrix< double, N, N, Eigen::RowMajor > m_measurementNoiseCovariance
The measurement noise covariance.
Definition: KalmanFilter.h:97
const Eigen::Matrix< double, M, 1 > & getState() const
Get the current state.
Definition: KalmanFilter-inl.h:96
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 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
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
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).
Definition: KalmanFilter-inl.h:102
Eigen::Matrix< double, M, 1 > m_state
The state.
Definition: KalmanFilter.h:100
Definitions of small fixed-size square matrix types.
Eigen::Matrix< double, M, M, Eigen::RowMajor > m_stateCovariance
The covariance of the state.
Definition: KalmanFilter.h:103
Definitions of small fixed-size vector types.
Eigen::Matrix< double, M, M, Eigen::RowMajor > m_processNoiseCovariance
The process noise covariance.
Definition: KalmanFilter.h:94
virtual ~KalmanFilter()
Destructor.
Definition: KalmanFilter-inl.h:39
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 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
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
Eigen::Matrix< double, M, M, Eigen::RowMajor > m_stateTransition
The state transition matrix.
Definition: KalmanFilter.h:88
const Eigen::Matrix< double, M, 1 > & update()
Advance one step without a measurement.
Definition: KalmanFilter-inl.h:80