KalmanFilter.h

Go to the documentation of this file.

// *****************************************************************
// Filename:    KalmanFilter.h
// Copyright:   Kai Welke, Chair Prof. Dillmann (IAIM),
//              Institute for Computer Science and Engineering (CSE),
//              University of Karlsruhe. All rights reserved.
// Author:      Kai Welke
// Date:        27.05.2009
// *****************************************************************
#pragma once
 
// *****************************************************************
// includes
// *****************************************************************
#include <math.h>
 
#include <Eigen/Eigen>
 
#include "Gaussian.h"
 
// Kalman filter for linear system with the following control model:
//      xt = Axt-1 + But + d + gaussian error
// and following measurment model
//      zt = Cx + e + gaussian error
 
// *****************************************************************
// declaration of KalmanFilter
// *****************************************************************
class KalmanFilter
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    // construction / destruction
    KalmanFilter(){};
 
    // setup system
    /**
     * @param motion_A nxn square matrix being the linear transformation of the state
     * @param motion_B nxm matrix being the transformation from control to state
     * @param motion_noise n dimensional gaussian being the motion noise
     **/
    void setMotionModel(const Eigen::MatrixXd& motion_A,
                        const Eigen::MatrixXd& motion_B,
                        const Gaussian& motion_noise);
 
    /**
     * @param measurement_C  transformation from state space to observation space
     * @param measurement_noise gaussian encoding the observation noise
     **/
    void setMeasurementModel(const Eigen::MatrixXd& measurement_C,
                             const Gaussian& measurement_noise);
 
    // filtering (if result == NULL, believe is updated)
    Gaussian filter(const Gaussian& believe,
                    const Eigen::VectorXd& actionU,
                    const Eigen::VectorXd& perceptZ);
    Gaussian predict(const Gaussian& believe, const Eigen::VectorXd& actionU);
    Gaussian update(const Gaussian& predicted_believe, const Eigen::VectorXd& perceptZ);
 
public:
    // system model
    Eigen::MatrixXd m_A;
    Eigen::MatrixXd m_B;
    Eigen::MatrixXd m_C;
    Eigen::MatrixXd m_A_t;
    Eigen::MatrixXd m_C_t;
 
    // noises
    Gaussian m_Q;
    Gaussian m_R;
};