KalmanFilter.cpp

Go to the documentation of this file.

#include <iostream>

#include "inference/model/KalmanFilter.h"

namespace ProbabilisticSceneRecognition {
    KalmanFilter::KalmanFilter(ISM::Object pObject) :

                        mReset(true)
        {
                mF = Eigen::MatrixXd::Identity(7, 7);
                mH = Eigen::MatrixXd::Identity(7, 7);

                mQ = 0.1 * Eigen::MatrixXd::Identity(7, 7);
                mR = 1 * Eigen::MatrixXd::Identity(7, 7);
                
                mP = Eigen::MatrixXd::Identity(mF.rows(), mF.cols());
                
                update(pObject);
        }

        // dtor
        KalmanFilter::~KalmanFilter() {
        }

        void KalmanFilter::reset() {
                mReset = true;
        }

    void KalmanFilter::update(ISM::Object pObject) {
          

                // Bring last update time uptodate.
                lastUpdate = std::chrono::high_resolution_clock::now();
          
                // Rename the mesaurement to x.
                Eigen::VectorXd x = Eigen::VectorXd::Zero(7);


        if(!pObject.pose){
          std::cerr << "Got a Object without poses." << std::endl;
                  std::exit(1);    
                }

        boost::shared_ptr<ISM::Pose> current_pose = pObject.pose;
        x(0) = current_pose->point->getEigen().x();
        x(1) = current_pose->point->getEigen().y();
        x(2) = current_pose->point->getEigen().z();
        x(3) = current_pose->quat->getEigen().w();
        x(4) = current_pose->quat->getEigen().x();
        x(5) = current_pose->quat->getEigen().y();
        x(6) = current_pose->quat->getEigen().z();
                
                // If reset flag is true, reset the system to the current measurement.
                if (mReset) {
                        mReset = false;
                        mX = x;
                        mZ = mH * x;
                }

                // Prediction step.
                Eigen::VectorXd xPred = mF * x;
                Eigen::MatrixXd PPred = mF * mP * mF.transpose() + mQ;

                // Update step
                Eigen::MatrixXd Ku = PPred * mH.transpose();
                Eigen::MatrixXd Kl = (mH * PPred * mH.transpose() + mR);
                Eigen::MatrixXd K = Ku * Kl.inverse();
                Eigen::VectorXd xNew = xPred + K * (mZ - mH * xPred);
                mP = PPred - K * mH * PPred;

                // Set the x and the z
                mX = xNew;
                mZ = mH * mX;

        // Save instance of Asr::AsrObject.
                mInstance = pObject;

        }
        
        bool KalmanFilter::isTimedOut(unsigned int threshold)
        {
          return std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - lastUpdate).count() > threshold;
        }

    ISM::Object KalmanFilter::getObject()
        {

          // Write the pose maintained by the kalman-filter back into the object.
      geometry_msgs::Pose current_pose;
      current_pose.position.x = mZ(0);
      current_pose.position.y = mZ(1);
      current_pose.position.z = mZ(2);
      current_pose.orientation.w = mZ(3);
      current_pose.orientation.x = mZ(4);
      current_pose.orientation.y = mZ(5);
      current_pose.orientation.z = mZ(6);

      //mInstance.sampledPoses.pop_back();
      geometry_msgs::PoseWithCovariance current_pose_with_c;
      current_pose_with_c.pose = current_pose;
     // mInstance.sampledPoses.push_back(current_pose_with_c);
          // Return the instance updated by the kalman filter.
          return mInstance;
        }
}