KFD_PosVelAcc.hpp

Go to the documentation of this file.

#ifndef __KFD_POS_VEL_ACC_HPP__
#define __KFD_POS_VEL_ACC_HPP__

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/SVD>
#include "KalmanFilter.hpp"

#include "FaultDetection.hpp"



namespace pose_ekf {
    class StatePosVelAcc
      {
    public:
        static const int SIZE = 9;
        typedef Eigen::Matrix<double,SIZE,1> vector_t;

    protected:
        vector_t _x; 
        Eigen::Block<vector_t, 3, 1> _pos_w;
        Eigen::Block<vector_t, 3, 1> _vel_i;
        Eigen::Block<vector_t, 3, 1> _acc_i;

    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        StatePosVelAcc() :
            _x(vector_t::Zero()), _pos_w( _x.segment<3>(0) ), _vel_i( _x.segment<3>(3) ),_acc_i(_x.segment<3>(6)) {};

        vector_t& vector() {return _x;};
        Eigen::Block<vector_t, 3, 1>& pos_world() {return _pos_w;}
        Eigen::Block<vector_t, 3, 1>& vel_world() {return _vel_i;}      
        Eigen::Block<vector_t, 3, 1>& acc_inertial() {return _acc_i;}

    };

    class KFD_PosVelAcc
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        
        static const unsigned int _POS_MEASUREMENT_SIZE = 3; 
        static const unsigned int _VEL_MEASUREMENT_SIZE = 3; 
        static const unsigned int _POS_DEGREE_OF_FREEDOM = 3;
        static const unsigned int _VEL_DEGREE_OF_FREEDOM = 3;

        
        StatePosVelAcc x;
        
    public:
        Eigen::Vector3d velocity_world; 
        Eigen::Vector3d position_world; 
        

        KalmanFilter::KF<StatePosVelAcc::SIZE>* filter;
        
        Eigen::Matrix<double, StatePosVelAcc::SIZE, StatePosVelAcc::SIZE> Q;
        
        Eigen::Quaterniond R_input_to_world; 
        
    public:
        KFD_PosVelAcc();
        ~KFD_PosVelAcc();
  
        void copyState ( const KFD_PosVelAcc& kfd ); 
        
        bool positionObservation( Eigen::Vector3d position, Eigen::Matrix3d covariance, float reject_position_threshol); 
        
        bool velocityObservation(Eigen::Vector3d velocity, Eigen::Matrix3d covariance, float reject_velocity_threshol);
        
        bool positionZObservation(double z, double error, double rejection_threshold) ;
        
        void setPosition( Eigen::Vector3d position, Eigen::Matrix3d covariance ); 

        void correct_state();
        
        void predict(Eigen::Vector3d acc, double dt, Eigen::Matrix<double, StatePosVelAcc::SIZE, StatePosVelAcc::SIZE> process_noise);
        
        void setRotation(Eigen::Quaterniond R); 
        
        Eigen::Quaterniond getRotation(){ return R_input_to_world; }
        
        Eigen::Vector3d getPosition(); 
        
        Eigen::Vector3d getVelocity();
        
        Eigen::Vector3d getAccBias(); 
        
        Eigen::Matrix3d getPositionCovariance(); 
        
        Eigen::Matrix3d getVelocityCovariance();
        
        Eigen::Matrix3d getAccCovariance();
        
        void init(const Eigen::Matrix<double, StatePosVelAcc::SIZE, StatePosVelAcc::SIZE> &P, const Eigen::Matrix<double,StatePosVelAcc::SIZE,1> &x); 


    
        
    };
}

#endif