KalmanFilter.h
Go to the documentation of this file.
1 // *****************************************************************
2 // Filename: KalmanFilter.h
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 #pragma once
10
11 // *****************************************************************
12 // includes
13 // *****************************************************************
14 #include <math.h>
15 #include <Eigen/Eigen>
16
17 #include "Gaussian.h"
18
19 // Kalman filter for linear system with the following control model:
20 // xt = Axt-1 + But + d + gaussian error
21 // and following measurment model
22 // zt = Cx + e + gaussian error
23
24 // *****************************************************************
25 // declaration of KalmanFilter
26 // *****************************************************************
28 {
29 public:
30  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
31
32  // construction / destruction
34
35  // setup system
36  /**
37  * @param motion_A nxn square matrix being the linear transformation of the state
38  * @param motion_B nxm matrix being the transformation from control to state
39  * @param motion_noise n dimensional gaussian being the motion noise
40  **/
41  void setMotionModel(const Eigen::MatrixXd& motion_A, const Eigen::MatrixXd& motion_B, const Gaussian& motion_noise);
42
43  /**
44  * @param measurement_C transformation from state space to observation space
45  * @param measurement_noise gaussian encoding the observation noise
46  **/
47  void setMeasurementModel(const Eigen::MatrixXd& measurement_C, const Gaussian& measurement_noise);
48
49  // filtering (if result == NULL, believe is updated)
50  Gaussian filter(const Gaussian& believe, const Eigen::VectorXd& actionU, const Eigen::VectorXd& perceptZ);
51  Gaussian predict(const Gaussian& believe, const Eigen::VectorXd& actionU);
52  Gaussian update(const Gaussian& predicted_believe, const Eigen::VectorXd& perceptZ);
53
54 public:
55  // system model
56  Eigen::MatrixXd m_A;
57  Eigen::MatrixXd m_B;
58  Eigen::MatrixXd m_C;
59  Eigen::MatrixXd m_A_t;
60  Eigen::MatrixXd m_C_t;
61
62  // noises
65 };
66
KalmanFilter::KalmanFilter
EIGEN_MAKE_ALIGNED_OPERATOR_NEW KalmanFilter()
Definition: KalmanFilter.h:33
KalmanFilter::setMotionModel
void setMotionModel(const Eigen::MatrixXd &motion_A, const Eigen::MatrixXd &motion_B, const Gaussian &motion_noise)
Definition: KalmanFilter.cpp:19
KalmanFilter
Definition: KalmanFilter.h:27
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
KalmanFilter::m_B
Eigen::MatrixXd m_B
Definition: KalmanFilter.h:57
KalmanFilter::m_C_t
Eigen::MatrixXd m_C_t
Definition: KalmanFilter.h:60
Gaussian.h
KalmanFilter::predict
Gaussian predict(const Gaussian &believe, const Eigen::VectorXd &actionU)
Definition: KalmanFilter.cpp:52
KalmanFilter::m_A
Eigen::MatrixXd m_A
Definition: KalmanFilter.h:56
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