KalmanFilter.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_H
17 #define SURGSIM_MATH_KALMANFILTER_H
18 
19 #include "SurgSim/Math/Matrix.h"
20 #include "SurgSim/Math/Vector.h"
21 
22 namespace SurgSim
23 {
24 namespace Math
25 {
26 
31 template <size_t M, size_t N>
33 {
34 public:
36  KalmanFilter();
37 
39  virtual ~KalmanFilter();
40 
43  void setInitialState(const Eigen::Ref<const Eigen::Matrix<double, M, 1>>& x);
44 
47  void setInitialStateCovariance(const Eigen::Ref<const Eigen::Matrix<double, M, M>>& p);
48 
51  void setStateTransition(const Eigen::Ref<const Eigen::Matrix<double, M, M>>& a);
52 
55  void setObservationMatrix(const Eigen::Ref<const Eigen::Matrix<double, N, M>>& h);
56 
59  void setProcessNoiseCovariance(const Eigen::Ref<const Eigen::Matrix<double, M, M>>& q);
60 
63  void setMeasurementNoiseCovariance(const Eigen::Ref<const Eigen::Matrix<double, N, N>>& r);
64 
67  const Eigen::Matrix<double, M, 1>& update();
68 
72  const Eigen::Matrix<double, M, 1>& update(const Eigen::Ref<const Eigen::Matrix<double, N, 1>>& measurement);
73 
76  const Eigen::Matrix<double, M, 1>& getState() const;
77 
78 private:
81  void updatePrediction();
82 
85  void updateMeasurement(const Eigen::Ref<const Eigen::Matrix<double, N, 1>>& measurement);
86 
88  Eigen::Matrix<double, M, M, Eigen::RowMajor> m_stateTransition;
89 
91  Eigen::Matrix<double, N, M, Eigen::RowMajor> m_observationMatrix;
92 
94  Eigen::Matrix<double, M, M, Eigen::RowMajor> m_processNoiseCovariance;
95 
97  Eigen::Matrix<double, N, N, Eigen::RowMajor> m_measurementNoiseCovariance;
98 
100  Eigen::Matrix<double, M, 1> m_state;
101 
103  Eigen::Matrix<double, M, M, Eigen::RowMajor> m_stateCovariance;
104 };
105 
106 }; // Math
107 }; // SurgSim
108 
110 
111 #endif // SURGSIM_MATH_KALMANFILTER_H
SurgSim::Math::KalmanFilter
Implements a linear Kalman filter, a recursive estimator.
Definition: KalmanFilter.h:33
Vector.h
Definitions of small fixed-size vector types.
SurgSim::Math::KalmanFilter::m_observationMatrix
Eigen::Matrix< double, N, M, Eigen::RowMajor > m_observationMatrix
The observation matrix.
Definition: KalmanFilter.h:91
SurgSim::Math::KalmanFilter::getState
const Eigen::Matrix< double, M, 1 > & getState() const
Get the current state.
Definition: KalmanFilter-inl.h:96
SurgSim::Math::KalmanFilter::m_processNoiseCovariance
Eigen::Matrix< double, M, M, Eigen::RowMajor > m_processNoiseCovariance
The process noise covariance.
Definition: KalmanFilter.h:94
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
Matrix.h
Definitions of small fixed-size square matrix types.
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:30
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::m_state
Eigen::Matrix< double, M, 1 > m_state
The state.
Definition: KalmanFilter.h:100
SurgSim::Math::KalmanFilter::m_measurementNoiseCovariance
Eigen::Matrix< double, N, N, Eigen::RowMajor > m_measurementNoiseCovariance
The measurement noise covariance.
Definition: KalmanFilter.h:97
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
KalmanFilter-inl.h
SurgSim::Math::KalmanFilter::m_stateCovariance
Eigen::Matrix< double, M, M, Eigen::RowMajor > m_stateCovariance
The covariance of the state.
Definition: KalmanFilter.h:103
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::KalmanFilter::m_stateTransition
Eigen::Matrix< double, M, M, Eigen::RowMajor > m_stateTransition
The state transition matrix.
Definition: KalmanFilter.h:88