KalmanFilter-inl.h
Go to the documentation of this file.
// This file is a part of the OpenSurgSim project.
// Copyright 2013, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef SURGSIM_MATH_KALMANFILTER_INL_H
#define SURGSIM_MATH_KALMANFILTER_INL_H
namespace SurgSim
{
namespace Math
{
template <size_t M, size_t N>
KalmanFilter<M, N>::KalmanFilter() :
m_stateTransition(Eigen::Matrix<double, M, M, Eigen::RowMajor>::Constant(std::numeric_limits<double>::quiet_NaN())),
m_observationMatrix(Eigen::Matrix<double, N, M, Eigen::RowMajor>::Constant(
std::numeric_limits<double>::quiet_NaN())),
m_processNoiseCovariance(Eigen::Matrix<double, M, M, Eigen::RowMajor>::Constant(
std::numeric_limits<double>::quiet_NaN())),
m_measurementNoiseCovariance(Eigen::Matrix<double, N, N, Eigen::RowMajor>::Constant(
std::numeric_limits<double>::quiet_NaN())),
m_state(Eigen::Matrix<double, M, 1>::Constant(std::numeric_limits<double>::quiet_NaN())),
m_stateCovariance(Eigen::Matrix<double, M, M, Eigen::RowMajor>::Constant(std::numeric_limits<double>::quiet_NaN()))
{
}
template <size_t M, size_t N>
KalmanFilter<M, N>::~KalmanFilter()
{
}
template <size_t M, size_t N>
void KalmanFilter<M, N>::setInitialState(const Eigen::Ref<const Eigen::Matrix<double, M, 1>>& x)
{
m_state = x;
}
template <size_t M, size_t N>
void KalmanFilter<M, N>::setInitialStateCovariance(const Eigen::Ref<const Eigen::Matrix<double, M, M>>& p)
{
m_stateCovariance = p;
}
template <size_t M, size_t N>
void KalmanFilter<M, N>::setStateTransition(const Eigen::Ref<const Eigen::Matrix<double, M, M>>& a)
{
m_stateTransition = a;
}
template <size_t M, size_t N>
void KalmanFilter<M, N>::setObservationMatrix(const Eigen::Ref<const Eigen::Matrix<double, N, M>>& h)
{
m_observationMatrix = h;
}
template <size_t M, size_t N>
void KalmanFilter<M, N>::setProcessNoiseCovariance(const Eigen::Ref<const Eigen::Matrix<double, M, M>>& q)
{
m_processNoiseCovariance = q;
}
template <size_t M, size_t N>
void KalmanFilter<M, N>::setMeasurementNoiseCovariance(const Eigen::Ref<const Eigen::Matrix<double, N, N>>& r)
{
m_measurementNoiseCovariance = r;
}
template <size_t M, size_t N>
const Eigen::Matrix<double, M, 1>& KalmanFilter<M, N>::update()
{
updatePrediction();
return m_state;
}
template <size_t M, size_t N>
const Eigen::Matrix<double, M, 1>&
KalmanFilter<M, N>::update(const Eigen::Ref<const Eigen::Matrix<double, N, 1>>& measurement)
{
updatePrediction();
updateMeasurement(measurement);
return m_state;
}
template <size_t M, size_t N>
const Eigen::Matrix<double, M, 1>& KalmanFilter<M, N>::getState() const
{
return m_state;
}
template <size_t M, size_t N>
void KalmanFilter<M, N>::updatePrediction()
{
m_state = m_stateTransition * m_state;
m_stateCovariance = m_stateTransition * m_stateCovariance * m_stateTransition.transpose() +
m_processNoiseCovariance;
}
template <size_t M, size_t N>
void KalmanFilter<M, N>::updateMeasurement(const Eigen::Ref<const Eigen::Matrix<double, N, 1>>& measurement)
{
const Matrix gain = m_stateCovariance * m_observationMatrix.transpose() *
(m_observationMatrix * m_stateCovariance * m_observationMatrix.transpose() +
m_measurementNoiseCovariance).inverse();
m_state += gain * (measurement - m_observationMatrix * m_state);
m_stateCovariance -= gain * m_observationMatrix * m_stateCovariance;
}
}; // namespace Math
}; // namespace SurgSim
#endif // SURGSIM_MATH_KALMANFILTER_INL_H
SurgSim
Definition: CompoundShapeToGraphics.cpp:29
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::m_observationMatrix
Eigen::Matrix< double, N, M, Eigen::RowMajor > m_observationMatrix
The observation matrix.
Definition: KalmanFilter.h:91
SurgSim::Math::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > Matrix
A dynamic size matrix.
Definition: Matrix.h:65
Eigen
Definition: MathUtilities.h:94
std
STL namespace.
SurgSim::Math::KalmanFilter::KalmanFilter
KalmanFilter()
Constructor.
Definition: KalmanFilter-inl.h:25
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::getState
const Eigen::Matrix< double, M, 1 > & getState() const
Get the current state.
Definition: KalmanFilter-inl.h:96
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::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::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::updatePrediction
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
SurgSim::Math::KalmanFilter::m_state
Eigen::Matrix< double, M, 1 > m_state
The state.
Definition: KalmanFilter.h:100
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::m_processNoiseCovariance
Eigen::Matrix< double, M, M, Eigen::RowMajor > m_processNoiseCovariance
The process noise covariance.
Definition: KalmanFilter.h:94
SurgSim::Math::KalmanFilter::~KalmanFilter
virtual ~KalmanFilter()
Destructor.
Definition: KalmanFilter-inl.h:39
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::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::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::m_stateTransition
Eigen::Matrix< double, M, M, Eigen::RowMajor > m_stateTransition
The state transition matrix.
Definition: KalmanFilter.h:88
SurgSim::Math::KalmanFilter::update
const Eigen::Matrix< double, M, 1 > & update()
Advance one step without a measurement.
Definition: KalmanFilter-inl.h:80