KalmanFilter-inl.h
Go to the documentation of this file.
1 // This file is a part of the OpenSurgSim project.
2 // Copyright 2013, SimQuest Solutions Inc.
3 //
4 // Licensed under the Apache License, Version 2.0 (the "License");
5 // you may not use this file except in compliance with the License.
6 // You may obtain a copy of the License at
7 //
8 // http://www.apache.org/licenses/LICENSE-2.0
9 //
10 // Unless required by applicable law or agreed to in writing, software
11 // distributed under the License is distributed on an "AS IS" BASIS,
12 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 // See the License for the specific language governing permissions and
14 // limitations under the License.
15 
16 #ifndef SURGSIM_MATH_KALMANFILTER_INL_H
17 #define SURGSIM_MATH_KALMANFILTER_INL_H
18 
19 namespace SurgSim
20 {
21 namespace Math
22 {
23 
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())),
27  m_observationMatrix(Eigen::Matrix<double, N, M, Eigen::RowMajor>::Constant(
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()))
35 {
36 }
37 
38 template <size_t M, size_t N>
40 {
41 }
42 
43 template <size_t M, size_t N>
44 void KalmanFilter<M, N>::setInitialState(const Eigen::Ref<const Eigen::Matrix<double, M, 1>>& x)
45 {
46  m_state = x;
47 }
48 
49 template <size_t M, size_t N>
50 void KalmanFilter<M, N>::setInitialStateCovariance(const Eigen::Ref<const Eigen::Matrix<double, M, M>>& p)
51 {
53 }
54 
55 template <size_t M, size_t N>
56 void KalmanFilter<M, N>::setStateTransition(const Eigen::Ref<const Eigen::Matrix<double, M, M>>& a)
57 {
59 }
60 
61 template <size_t M, size_t N>
62 void KalmanFilter<M, N>::setObservationMatrix(const Eigen::Ref<const Eigen::Matrix<double, N, M>>& h)
63 {
65 }
66 
67 template <size_t M, size_t N>
68 void KalmanFilter<M, N>::setProcessNoiseCovariance(const Eigen::Ref<const Eigen::Matrix<double, M, M>>& q)
69 {
71 }
72 
73 template <size_t M, size_t N>
74 void KalmanFilter<M, N>::setMeasurementNoiseCovariance(const Eigen::Ref<const Eigen::Matrix<double, N, N>>& r)
75 {
77 }
78 
79 template <size_t M, size_t N>
80 const Eigen::Matrix<double, M, 1>& KalmanFilter<M, N>::update()
81 {
83  return m_state;
84 }
85 
86 template <size_t M, size_t N>
87 const Eigen::Matrix<double, M, 1>&
88  KalmanFilter<M, N>::update(const Eigen::Ref<const Eigen::Matrix<double, N, 1>>& measurement)
89 {
91  updateMeasurement(measurement);
92  return m_state;
93 }
94 
95 template <size_t M, size_t N>
96 const Eigen::Matrix<double, M, 1>& KalmanFilter<M, N>::getState() const
97 {
98  return m_state;
99 }
100 
101 template <size_t M, size_t N>
103 {
107 }
108 
109 template <size_t M, size_t N>
110 void KalmanFilter<M, N>::updateMeasurement(const Eigen::Ref<const Eigen::Matrix<double, N, 1>>& measurement)
111 {
112  const Matrix gain = m_stateCovariance * m_observationMatrix.transpose() *
114  m_measurementNoiseCovariance).inverse();
115  m_state += gain * (measurement - m_observationMatrix * m_state);
117 }
118 
119 }; // namespace Math
120 }; // namespace SurgSim
121 
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
STL namespace.
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