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 {
52  m_stateCovariance = p;
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 {
58  m_stateTransition = a;
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 {
64  m_observationMatrix = h;
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 {
70  m_processNoiseCovariance = q;
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 {
76  m_measurementNoiseCovariance = r;
77 }
78 
79 template <size_t M, size_t N>
80 const Eigen::Matrix<double, M, 1>& KalmanFilter<M, N>::update()
81 {
82  updatePrediction();
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 {
90  updatePrediction();
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 {
104  m_state = m_stateTransition * m_state;
105  m_stateCovariance = m_stateTransition * m_stateCovariance * m_stateTransition.transpose() +
106  m_processNoiseCovariance;
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() *
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;
117 }
118 
119 }; // namespace Math
120 }; // namespace SurgSim
121 
122 #endif // SURGSIM_MATH_KALMANFILTER_INL_H
Eigen
Definition: MathUtilities.h:94
SurgSim::Math::KalmanFilter::getState
const Eigen::Matrix< double, M, 1 > & getState() const
Get the current state.
Definition: KalmanFilter-inl.h:96
SurgSim::Math::KalmanFilter::update
const Eigen::Matrix< double, M, 1 > & update()
Advance one step without a measurement.
Definition: KalmanFilter-inl.h:80
SurgSim::Math::KalmanFilter::setMeasurementNoiseCovariance
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
SurgSim::Math::KalmanFilter::setInitialStateCovariance
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
SurgSim
Definition: CompoundShapeToGraphics.cpp:29
SurgSim::Math::KalmanFilter::setObservationMatrix
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
SurgSim::Math::KalmanFilter::updatePrediction
void updatePrediction()
Use the current estimated state, x(t), and matrices to predict the new state, x(t+1),...
Definition: KalmanFilter-inl.h:102
SurgSim::Math::KalmanFilter::updateMeasurement
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
SurgSim::Math::KalmanFilter::KalmanFilter
KalmanFilter()
Constructor.
Definition: KalmanFilter-inl.h:25
SurgSim::Math::KalmanFilter::setStateTransition
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
SurgSim::Math::KalmanFilter::setInitialState
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
SurgSim::Math::KalmanFilter::setProcessNoiseCovariance
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
SurgSim::Math::KalmanFilter::~KalmanFilter
virtual ~KalmanFilter()
Destructor.
Definition: KalmanFilter-inl.h:39
SurgSim::Math::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > Matrix
A dynamic size matrix.
Definition: Matrix.h:65