KalmanFilter.cpp

Go to the documentation of this file.

// *****************************************************************
// Filename:    KalmanFilter.cpp
// 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
// *****************************************************************
 
// *****************************************************************
// includes
// *****************************************************************
#include "KalmanFilter.h"
 
#include <iostream>
 
// *****************************************************************
// setup
// *****************************************************************
void
KalmanFilter::setMotionModel(const Eigen::MatrixXd& motion_A,
                             const Eigen::MatrixXd& motion_B,
                             const Gaussian& motion_noise)
{
    m_A = motion_A;
    m_B = motion_B;
 
    m_R = motion_noise;
 
    m_A_t = m_A.transpose();
}
 
void
KalmanFilter::setMeasurementModel(const Eigen::MatrixXd& measurement_C,
                                  const Gaussian& measurement_noise)
{
    m_C = measurement_C;
 
    m_Q = measurement_noise;
 
    m_C_t = m_C.transpose();
}
 
// *****************************************************************
// prediction / update
// *****************************************************************
Gaussian
KalmanFilter::filter(const Gaussian& believe,
                     const Eigen::VectorXd& actionU,
                     const Eigen::VectorXd& perceptZ)
{
    // prediction
    Gaussian bel_predicted = predict(believe, actionU);
 
    // update
    Gaussian result = update(bel_predicted, perceptZ);
 
    return result;
}
 
Gaussian
KalmanFilter::predict(const Gaussian& believe, const Eigen::VectorXd& actionU)
{
    // predict step bel_predicted = p(xt|xt-1,ut)
    Gaussian predicted(believe.getDimensions());
 
    predicted.setMean(m_A * believe.getMean() + m_B * actionU);
    predicted.setCovariance(m_A * believe.getCovariance() * m_A_t + m_R.getCovariance());
 
    return predicted;
}
 
Gaussian
KalmanFilter::update(const Gaussian& predicted_believe, const Eigen::VectorXd& perceptZ)
{
    // calculate Kalman gain
    //@@@ TODO FIX: if temp ~= 0 temp.inverse() is not possible => temp.inverse() contains nan.
    //std::cout << "***********m_Q.getCovariance():" << m_Q.getCovariance() << std::endl;
    Eigen::MatrixXd temp = m_C * predicted_believe.getCovariance() * m_C_t + m_Q.getCovariance();
    //std::cout << "***********temp:" << temp << std::endl;
    //std::cout << "***********temp.inverse():" << temp.inverse() << std::endl;
    //std::cout << "***********predicted_believe.getCovariance():" << predicted_believe.getCovariance() << std::endl;
    //std::cout << "***********m_C_t:" << m_C_t << std::endl;
    Eigen::MatrixXd gain = predicted_believe.getCovariance() * m_C_t * temp.inverse();
    //std::cout << "***********gain:" << gain << std::endl;
 
    // update step bel = lambda * p(zt|xt) * bel_prior
    Gaussian result(predicted_believe.getDimensions());
    result.setMean(predicted_believe.getMean() +
                   gain * (perceptZ - m_C * predicted_believe.getMean()));
    //    result.setCovariance((Eigen::MatrixXd::Identity(gain.rows(),gain.cols()) - gain * m_C) * predicted_believe.getCovariance());
 
    result.setCovariance(predicted_believe.getCovariance() - gain * temp * gain.transpose());
 
    return result;
}