KalmanFilter.cpp
Go to the documentation of this file.
1 // *****************************************************************
2 // Filename: KalmanFilter.cpp
3 // Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
4 // Institute for Computer Science and Engineering (CSE),
6 // Author: Kai Welke
7 // Date: 27.05.2009
8 // *****************************************************************
9
10 // *****************************************************************
11 // includes
12 // *****************************************************************
13 #include "KalmanFilter.h"
14 #include <iostream>
15
16 // *****************************************************************
17 // setup
18 // *****************************************************************
19 void KalmanFilter::setMotionModel(const Eigen::MatrixXd& motion_A, const Eigen::MatrixXd& motion_B, const Gaussian& motion_noise)
20 {
21  m_A = motion_A;
22  m_B = motion_B;
23
24  m_R = motion_noise;
25
26  m_A_t = m_A.transpose();
27 }
28
29 void KalmanFilter::setMeasurementModel(const Eigen::MatrixXd& measurement_C, const Gaussian& measurement_noise)
30 {
31  m_C = measurement_C;
32
33  m_Q = measurement_noise;
34
35  m_C_t = m_C.transpose();
36 }
37
38 // *****************************************************************
39 // prediction / update
40 // *****************************************************************
41 Gaussian KalmanFilter::filter(const Gaussian& believe, const Eigen::VectorXd& actionU, const Eigen::VectorXd& perceptZ)
42 {
43  // prediction
44  Gaussian bel_predicted = predict(believe, actionU);
45
46  // update
47  Gaussian result = update(bel_predicted, perceptZ);
48
49  return result;
50 }
51
52 Gaussian KalmanFilter::predict(const Gaussian& believe, const Eigen::VectorXd& actionU)
53 {
54  // predict step bel_predicted = p(xt|xt-1,ut)
55  Gaussian predicted(believe.getDimensions());
56
57  predicted.setMean(m_A * believe.getMean() + m_B * actionU);
58  predicted.setCovariance(m_A * believe.getCovariance() * m_A_t + m_R.getCovariance());
59
60  return predicted;
61 }
62
63 Gaussian KalmanFilter::update(const Gaussian& predicted_believe, const Eigen::VectorXd& perceptZ)
64 {
65  // calculate Kalman gain
66  //@@@ TODO FIX: if temp ~= 0 temp.inverse() is not possible => temp.inverse() contains nan.
67  //std::cout << "***********m_Q.getCovariance():" << m_Q.getCovariance() << std::endl;
68  Eigen::MatrixXd temp = m_C * predicted_believe.getCovariance() * m_C_t + m_Q.getCovariance();
69  //std::cout << "***********temp:" << temp << std::endl;
70  //std::cout << "***********temp.inverse():" << temp.inverse() << std::endl;
71  //std::cout << "***********predicted_believe.getCovariance():" << predicted_believe.getCovariance() << std::endl;
72  //std::cout << "***********m_C_t:" << m_C_t << std::endl;
73  Eigen::MatrixXd gain = predicted_believe.getCovariance() * m_C_t * temp.inverse();
74  //std::cout << "***********gain:" << gain << std::endl;
75
76  // update step bel = lambda * p(zt|xt) * bel_prior
77  Gaussian result(predicted_believe.getDimensions());
78  result.setMean(predicted_believe.getMean() + gain * (perceptZ - m_C * predicted_believe.getMean()));
79  // result.setCovariance((Eigen::MatrixXd::Identity(gain.rows(),gain.cols()) - gain * m_C) * predicted_believe.getCovariance());
80
81  result.setCovariance(predicted_believe.getCovariance() - gain * temp * gain.transpose());
82
83  return result;
84 }
Gaussian::getCovariance
const covariance_type & getCovariance() const
Definition: Gaussian.h:77
KalmanFilter::setMotionModel
void setMotionModel(const Eigen::MatrixXd &motion_A, const Eigen::MatrixXd &motion_B, const Gaussian &motion_noise)
Definition: KalmanFilter.cpp:19
KalmanFilter::setMeasurementModel
void setMeasurementModel(const Eigen::MatrixXd &measurement_C, const Gaussian &measurement_noise)
Definition: KalmanFilter.cpp:29
KalmanFilter::m_C
Eigen::MatrixXd m_C
Definition: KalmanFilter.h:58
KalmanFilter::filter
Gaussian filter(const Gaussian &believe, const Eigen::VectorXd &actionU, const Eigen::VectorXd &perceptZ)
Definition: KalmanFilter.cpp:41
Gaussian::getDimensions
int getDimensions() const
Definition: Gaussian.h:85
KalmanFilter::m_B
Eigen::MatrixXd m_B
Definition: KalmanFilter.h:57
Gaussian::setMean
void setMean(const value_type &mean)
Definition: Gaussian.cpp:245
KalmanFilter::m_C_t
Eigen::MatrixXd m_C_t
Definition: KalmanFilter.h:60
Gaussian::setCovariance
void setCovariance(const covariance_type &cov)
Definition: Gaussian.cpp:261
KalmanFilter::predict
Gaussian predict(const Gaussian &believe, const Eigen::VectorXd &actionU)
Definition: KalmanFilter.cpp:52
KalmanFilter.h
KalmanFilter::m_A
Eigen::MatrixXd m_A
Definition: KalmanFilter.h:56
Gaussian::getMean
const value_type & getMean() const
Definition: Gaussian.h:81
Gaussian
Definition: Gaussian.h:46
KalmanFilter::m_Q
Gaussian m_Q
Definition: KalmanFilter.h:63
KalmanFilter::m_R
Gaussian m_R
Definition: KalmanFilter.h:64
KalmanFilter::update
Gaussian update(const Gaussian &predicted_believe, const Eigen::VectorXd &perceptZ)
Definition: KalmanFilter.cpp:63
KalmanFilter::m_A_t
Eigen::MatrixXd m_A_t
Definition: KalmanFilter.h:59