ukf.hpp

Go to the documentation of this file.

#ifndef _FILTER_UKF_HPP_
#define _FILTER_UKF_HPP_

#include <iostream>
#include <Eigen/Geometry> 
namespace filter
{
    
    using namespace Eigen;

    class ukf
    {
        public:
            static const int  UKFSTATEVECTORSIZE = 6; 
            static const int  QUATERSIZE = 4; 
            static const int  SIGPOINTSIZE =  (2*ukf::UKFSTATEVECTORSIZE) + 1; 
            static const int  NUMAXIS = 3; 
            enum DECLINATION_CONSTS {
                EAST = 1, 
                WEST = 2 
            };

        
        private:
            double f, a, lambda; 
            Eigen::Matrix <double,ukf::UKFSTATEVECTORSIZE,1> x; 
            Eigen::Matrix <double,ukf::NUMAXIS,1> gtilde; 
            Eigen::Matrix <double,ukf::UKFSTATEVECTORSIZE, ukf::UKFSTATEVECTORSIZE> Px; 
            Eigen::Quaternion <double> at_q;  
            Eigen::Matrix <double,ukf::UKFSTATEVECTORSIZE, ukf::UKFSTATEVECTORSIZE> Q; 
            Eigen::Matrix <double,ukf::NUMAXIS, ukf::NUMAXIS>  R; 
            Eigen::Matrix <double,ukf::UKFSTATEVECTORSIZE, ukf::SIGPOINTSIZE> sig_point; 
            Eigen::Matrix <Eigen::Quaternion <double>, ukf::SIGPOINTSIZE, 1> e_q; 
            Eigen::Matrix <Eigen::Quaternion <double>, ukf::SIGPOINTSIZE, 1> sig_q; 
            Eigen::Matrix <double,ukf::NUMAXIS, ukf::SIGPOINTSIZE> gamma; 
            Eigen::Matrix <double,ukf::NUMAXIS, 1> z_e; 
            Eigen::Matrix <double,ukf::NUMAXIS, 1> z_r; 
            Eigen::Matrix <double,ukf::NUMAXIS, 1> Nu; 
            Eigen::Matrix <double,ukf::NUMAXIS, ukf::NUMAXIS> Pzz; 
            Eigen::Matrix <double,ukf::NUMAXIS, ukf::NUMAXIS> Pnu; 
            Eigen::Matrix <double,ukf::UKFSTATEVECTORSIZE, ukf::NUMAXIS> Pxz; 
            Eigen::Matrix <double,ukf::UKFSTATEVECTORSIZE, ukf::NUMAXIS> K; 
        public: 
            
            
            Eigen::Matrix <double,ukf::UKFSTATEVECTORSIZE,1> getState();


            Eigen::Quaternion <double> getAttitude();

            Eigen::Matrix <double, ukf::NUMAXIS, 1> getEuler();

            Eigen::Matrix <double,ukf::UKFSTATEVECTORSIZE,ukf::UKFSTATEVECTORSIZE> getCovariance();

            bool setAttitude (Eigen::Quaternion <double> *initq);

                    
            void Init (Matrix <double,ukf::UKFSTATEVECTORSIZE,1> *x_0, Eigen::Matrix <double, ukf::UKFSTATEVECTORSIZE, ukf::UKFSTATEVECTORSIZE> *P_0, Eigen::Matrix <double, ukf::UKFSTATEVECTORSIZE, ukf::UKFSTATEVECTORSIZE> *Q, Eigen::Matrix <double, ukf::NUMAXIS, ukf::NUMAXIS> *R,
                       Eigen::Quaternion <double> *at_q, double a, double f, double lambda, double g);
            
            double GravityModel (double latitude, double altitude);
            
            void SubstractEarthRotation(Eigen::Matrix <double, ukf::NUMAXIS, 1> *u, Eigen::Quaternion <double> *qb_g, double latitude);
      
            void Omega(Eigen::Quaternion< double >* quat, Eigen::Matrix< double, ukf::NUMAXIS , 1  >* angvelo, double dt);

            
            void predict(Eigen::Matrix <double,ukf::NUMAXIS,1>  *u, double dt);
            
            
            
            void update(Eigen::Matrix <double,ukf::NUMAXIS,1>  *acc, Eigen::Matrix <double,ukf::NUMAXIS,1>  *mag);
            
            void attitudeUpdate();
            
        

    };

} // end namespace dummy_project

#endif // _FILTER_UKF_HPP_