ConstantVelocityKalmanFilter.hpp

# ifndef CONST_VELOCITY_KF_H
# define CONST_VELOCITY_KF_H

#include <iostream>

namespace jafar {
  namespace filter {
    

    class ConstantVelocityKalmanFilter{
    public:
      ConstantVelocityKalmanFilter();
      
      void initKF(double initPos, double initVar);
      
      void updateKF(double y, double sigmaY, double sigmaV);
      
      void getPred(double& pos, double& c11, double& c12, double& c22){
        pos = x1Pred;
        c11 = cov11Pred;
        c12 = cov12Pred;
        c22 = cov22Pred;
      }
      
      double getPosPred(){return x1Pred;}
      double getPosVar(){return cov11;}
      
      void setPosPeriod(double min, double max){
        minPeriod = min;
        maxPeriod = max;
        periodicFlag = 1;
      }
      
    private:
      double x1;    
      double x2;    
    
      double cov11; 
      double cov12; 
      double cov22; 
    
      double x1Pred;    
      double x2Pred;    
      
      double cov11Pred; 
      double cov12Pred; 
      double cov22Pred; 
      
      double minPeriod;   
      double maxPeriod;   
      bool periodicFlag;  
    };
  } // namespace lines
} // namespace jafar
# endif