gps_acc_fusion_filter.h

Go to the documentation of this file.

#ifndef GPSACCKALMAN_H
#define GPSACCKALMAN_H
 
#include "kalman.h"
 
struct kf_state {
  double x;      
  double y;      
  double x_vel;  
  double y_vel;  
 
  kf_state() : x(0.0), y(0.0), x_vel(0.0), y_vel(0.0) {}
  kf_state(double x_, double y_, double x_vel_, double y_vel_)
      : x(x_), y(y_), x_vel(x_vel_), y_vel(y_vel_)
  {
  }
 
  friend std::ostream& operator<<(std::ostream& os, const kf_state& obj);
};
 
 
class gps_acc_fusion_filter : public kalman_filter<4, 4, 2>
{
 private:
  double m_last_predict_sec; 
  double m_acc_sigma_2;  
  double m_pos_sigma_2;  
 
  void rebuild_F(double dt_sec);
  void rebuild_U(double xAcc, double yAcc);
  void rebuild_B(double dt_sec);
  void rebuild_Q(double dt_sec);
  void rebuild_R(double pos_sigma_2, double vel_sigma_2);
 
 public:
  gps_acc_fusion_filter();
 
  void reset(double x,   // EAST - longitude in meters
             double y,   // NORTH - latitude in meters
             double ts,  // last update (GPS coordinate) time
             double x_vel,
             double y_vel,
             double acc_sigma_2,
             double pos_sigma_2);
 
  void predict(double x_acc, double y_acc, double ts_sec);
  void update(const kf_state& state,
              double pos_sigma_2,
              double vel_sigma_2 = 1e-6);
 
  const kf_state current_state() const;
};
 
#endif  // GPSACCKALMAN_H