KFD_PosVelOriAcc.hpp

Go to the documentation of this file.

#ifndef __KFD_POS_VEL_ORI_ACC_HPP__
#define __KFD_POS_VEL_ORI_ACC_HPP__

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

#include "FaultDetection.hpp"



namespace pose_ekf {
    class StatePosVelOriAcc
      {
    public:
        static const int SIZE = 15;
        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;
        Eigen::Block<vector_t, 3, 1> _or_w;
        Eigen::Block<vector_t, 3, 1> _w_i;

    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        StatePosVelOriAcc() :
            _x(vector_t::Zero()), _pos_w( _x.segment<3>(0) ), _vel_i( _x.segment<3>(3) ),_acc_i(_x.segment<3>(6)), _or_w(_x.segment<3>(9)), _w_i(_x.segment<3>(12)) {};

        vector_t& vector() {return _x;};
        Eigen::Block<vector_t, 3, 1>& pos_world() {return _pos_w;}
        Eigen::Block<vector_t, 3, 1>& vel_inertial() {return _vel_i;}   
        Eigen::Block<vector_t, 3, 1>& acc_inertial() {return _acc_i;}
        Eigen::Block<vector_t, 3, 1>& or_w() {return _or_w;}
        Eigen::Block<vector_t, 3, 1>& angular_velocity_inertial() {return _w_i;}
    };

    class KFD_PosVelOriAcc
    {
    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;
        static const unsigned int _ORI_MEASUREMENT_SIZE = 3; 
        static const unsigned int _ORI_DEGREE_OF_FREEDOM = 3; 
        
        StatePosVelOriAcc x;
        
        //Eigen::Matrix<double, StatePosVelOriAcc::SIZE, StatePosVelOriAcc::SIZE> P;
        
    public:
        Eigen::Vector3d velocity_inertial; 
        Eigen::Vector3d position_world; 
        Eigen::Quaterniond R_inertial_2_world; 
        

        
        KalmanFilter::KF<StatePosVelOriAcc::SIZE>* filter;
        
        Eigen::Matrix<double, StatePosVelOriAcc::SIZE, StatePosVelOriAcc::SIZE> Q;
        
        
    public:
        KFD_PosVelOriAcc();
        ~KFD_PosVelOriAcc();
  
        void copyState ( const KFD_PosVelOriAcc& kfd ); 
        
        bool positionObservation( Eigen::Vector3d position, Eigen::Matrix3d covariance, float reject_position_threshol); 
        
        bool velocityObservation(Eigen::Vector3d velocity_body, Eigen::Matrix3d covariance, float reject_velocity_threshol);
        
        bool orientationObservation( Eigen::Quaterniond orientation, Eigen::Matrix3d covariance, float reject_orientation_threshol);
        
        void setPosition( Eigen::Vector3d position, Eigen::Matrix3d covariance ); 

        void setOrientation( Eigen::Quaterniond orientation, Eigen::Matrix3d covariance );
        
        void correct_state();
        
        void predict(Eigen::Vector3d acc_intertial, Eigen::Vector3d angular_velocity, double dt,Eigen::Matrix<double, StatePosVelOriAcc::SIZE, StatePosVelOriAcc::SIZE> process_noise);
        
        Eigen::Vector3d getPosition(); 
        
        Eigen::Vector3d getVelocity();
        
        Eigen::Quaterniond getOrientationInertial2World(); 
        
        Eigen::Matrix3d getPositionCovariance(); 
        
        Eigen::Matrix3d getVelocityCovariance();
        
        Eigen::Matrix3d getOrientationCovariance();
        
        Eigen::Quaterniond angularCorrection();
      
        void init(const Eigen::Matrix<double, StatePosVelOriAcc::SIZE, StatePosVelOriAcc::SIZE> &P, const Eigen::Matrix<double,StatePosVelOriAcc::SIZE,1> &x); 


    
        
    };
}

#endif