fusion.hpp

#ifndef FUSION_FUSION_HPP
#define FUSION_FUSION_HPP

#include "kernel/jafarException.hpp"
#include "jmath/jblas.hpp"
#include "filter/kalmanFilter.hpp"
#include "filter/predictModel.hpp"
#include "filter/observeModel.hpp"

#include "datareader/DataReader.hpp"

#include "boost/numeric/ublas/io.hpp"
#include "boost/numeric/ublas/lu.hpp" //to use project function
#include "boost/date_time/posix_time/posix_time_types.hpp"
#include <iostream>
#include <iomanip>
#include <fstream>
#include <math.h>

namespace jafar
{
   namespace fusion
   {
      class PredictModelFusion : public filter::JacobianCommandPredictModel 
      {
         public:
            PredictModelFusion(std::size_t sizeState, std::size_t sizeCommand);
            virtual ~PredictModelFusion();
            virtual void predict(jblas::vec& x, jblas::vec const& u_);
            void computeJacobianState(jblas::vec _x,
                                                            jblas::vec u,
                                                            jblas::mat C,
                                                            jblas::mat E);
            virtual void setTimeStep(boost::posix_time::time_duration const& timeStep)
            {
                m_timeStep = timeStep;
            }

          private:
           //the covariance matrix of constrol noise
           jblas::mat DCMmatrix(jblas::vec& _x);
           jblas::mat RotationRateMatrix(jblas::vec& _x);
           jblas::sym_mat W;
           
      };//the predict model class inheritated from the class JacobianPredictModel
      
      class ObservationModelFusion : public filter::JacobianObserveModel
      {
         public:
         ObservationModelFusion(std::size_t sizeObs_, std::size_t sizeState_, int sensor);
         virtual ~ObservationModelFusion();
         jblas::vec const& predictObservation(jblas::vec const& x_);
         virtual void predictObservationJac(const jblas::vec& x_);
         void setUncorrelatedR(jblas::vec R_); 
         void setSampleTime(double time){m_samplingTime = time;};
         double getSampleTime(){return m_samplingTime;};
         void setObsNum(std::size_t num){m_obsNum = num;};
          jblas::vec eulerPrev; //store the previous euler representation
         private:
                int m_sensor;  //set sensor type 
                std::size_t m_obsNum;  //set observation flag. 2: constraints 6: vision
                double m_samplingTime;
                
      }; //the observation model class inheritated from the class JacobianObserveModel
           
      /*
      * EKF fusion class
      */
      class ExtendedKalmanFilterFusion : public filter::ExtendedKalmanFilter
      {
         public:
         ExtendedKalmanFilterFusion(std::size_t n_);
        
         virtual ~ExtendedKalmanFilterFusion();
         
         void predict(PredictModelFusion& m,
                               jblas::vec const& u) ;
             
         void update(ObservationModelFusion& m_, const jblas::vec& z_);
         
         void writeHeader(kernel::DataLogger& log);
         void writeData(kernel::DataLogger& log);
         void writeVarData(kernel::DataLogger& log);
         double NormalizeAngle(double a);
         jblas::vec m_eulerPrev;   //store the euler representation at previous time
         void WriteVecBody(kernel::DataLogger& log);
     
      };//the main ekf fusion class

       //the file loaded class for fusion class
       class DataLoad 
       {
            public:
                DataLoad(datareader::DataReader *dr);
                virtual ~DataLoad();
                jblas::vec readImuLine();
                double readVisionDate(int index);
                double getStartImuTime(){return m_startTime;};
            private:
                int m_ColumnNum;
                std::ifstream m_imuLog;
                std::string m_path;
                double  m_startTime;  //initial second of IMU 
        };

   }
}
#endif