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>
55 template <
size_t M,
size_t N>
61 template <
size_t M,
size_t N>
67 template <
size_t M,
size_t N>
73 template <
size_t M,
size_t N>
79 template <
size_t M,
size_t N>
86 template <
size_t M,
size_t N>
87 const Eigen::Matrix<double, M, 1>&
95 template <
size_t M,
size_t N>
101 template <
size_t M,
size_t N>
109 template <
size_t M,
size_t N>
122 #endif // SURGSIM_MATH_KALMANFILTER_INL_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
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > Matrix
A dynamic size matrix.
Definition: Matrix.h:65
Definition: MathUtilities.h:94
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
Eigen::Matrix< double, M, M, Eigen::RowMajor > m_stateCovariance
The covariance of the state.
Definition: KalmanFilter.h:103
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