EKFPosYawBiasT.hpp

Go to the documentation of this file.

#ifndef __EKF_POSITIONYAWBIAS_HPP__
#define __EKF_POSITIONYAWBIAS_HPP__

#include <Eigen/Core>
#include <Eigen/Geometry>
#include "ExtendedKalmanFilter.hpp"

#include "FaultDetection.hpp"


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

    protected:
        vector_t _x; 
        Eigen::Block<vector_t, 3, 1> _xi;
        Eigen::Block<vector_t, 1, 1> _yaw;

    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        StatePosYawBias() :
            _x(vector_t::Zero()), _xi( _x.segment<3>(0) ),_yaw(_x.segment<1>(3)) {};

        vector_t& vector() {return _x;};
        Eigen::Block<vector_t, 3, 1>& xi() {return _xi;}        
        Eigen::Block<vector_t, 1, 1>& yaw() {return _yaw;}
    };

    class EKFPosYawBiasT
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        
        StatePosYawBias x;
        
    private:
        

        
        ExtendedKalmanFilter::EKF<StatePosYawBias::SIZE>* filter;
        
        Eigen::Matrix<double, StatePosYawBias::SIZE, StatePosYawBias::SIZE> Q;
        
        
    public:

        EKFPosYawBiasT();
        ~EKFPosYawBiasT();
  
        void copyState ( const EKFPosYawBiasT& kfd ); 
        
        void predict(const Eigen::Vector3d &translation_world, const Eigen::Matrix<double, StatePosYawBias::SIZE, StatePosYawBias::SIZE> &Q);

        void init(const Eigen::Matrix<double, StatePosYawBias::SIZE, StatePosYawBias::SIZE> &P, const Eigen::Matrix<double,StatePosYawBias::SIZE,1> &x); 
        
        Eigen::Quaterniond getOrientationCorrection();
        Eigen::Matrix3d getCovariancePosition(); 
        Eigen::Vector3d getPosition();
        double getOrientationCorrectionCovariance();

        void setInitialPosition( const Eigen::Vector3d &position, const Eigen::Matrix3d &covariance ); 
        
        bool correctPosition( const Eigen::Vector3d &position, const Eigen::Matrix3d &covariance, float reject_threshold ); 

        bool correctPositionOrientation( const Eigen::Vector4d &positionOrientation, const Eigen::Matrix4d &covariance, float reject_threshold ); 
        
      private: 
        
        Eigen::Matrix<double, StatePosYawBias::SIZE, StatePosYawBias::SIZE> jacobianF( const Eigen::Vector3d &translation_world );
        
        
    };
}

#endif