1  // %flair:license{


2  // This file is part of the Flair framework distributed under the


3  // CECILLC License, Version 1.0.


4  // %flair:license}


5  /*!


6  * \file AhrsKalman_impl.h


7  * \brief Class defining an Ahrs Kalman filter


8  * \author Tom Pycke, API changes by Guillaume Sanahuja to fit the Flair framework


9  * \date 2007/10/17, 2014/01/15


10  * \version


11  */


12 


13  #ifndef AHRSKALMAN_IMPL_H


14  #define AHRSKALMAN_IMPL_H


15 


16  #include <Ahrs.h>


17  #include <Euler.h>


18 


19  namespace flair {


20  namespace core {


21  class AhrsData;


22  }


23  namespace gui {


24  class Layout;


25  class DoubleSpinBox;


26  }


27  }


28 


29  /*! \class AhrsKalman_impl


30  * \brief Class defining an Ahrs Kalman filter


31  */


32 


33  class AhrsKalman_impl {


34  public:


35  AhrsKalman_impl(const flair::gui::Layout *layout,flair::core::AhrsData *ahrsData);


36  ~AhrsKalman_impl();


37  void UpdateFrom(const flair::core::io_data *data);


38 


39  private:


40  flair::core::AhrsData *ahrsData;


41  typedef struct ars_Gyro1DKalman


42  {


43  /* These variables represent our state matrix x */


44  double x_angle,


45  x_bias;


46 


47  /* Our error covariance matrix */


48  double P_00,


49  P_01,


50  P_10,


51  P_11;


52 


53  /*


54  * Q is a 2x2 matrix of the covariance. Because we


55  * assume the gyro and accelero noise to be independend


56  * of eachother, the covariances on the / diagonal are 0.


57  *


58  * Covariance Q, the process noise, from the assumption


59  * x = F x + B u + w


60  * with w having a normal distribution with covariance Q.


61  * (covariance = E[ (X  E[X])*(X  E[X])' ]


62  * We assume is linair with dt


63  */


64  double Q_angle, Q_gyro;


65  /*


66  * Covariance R, our observation noise (from the accelerometer)


67  * Also assumed to be linair with dt


68  */


69  double R_angle;


70  } ars_Gyro1DKalman;


71 


72  // Initializing the struct


73  void ars_Init(ars_Gyro1DKalman *filterdata, double Q_angle, double Q_gyro, double R_angle);


74  // Kalman predict


75  void ars_predict(ars_Gyro1DKalman *filterdata, const double gyro, const double dt);


76  // Kalman update


77  void ars_update(ars_Gyro1DKalman *filterdata, const double angle_m);


78 


79  ars_Gyro1DKalman ars_roll;


80  ars_Gyro1DKalman ars_pitch;


81  flair::core::Time previous_time;


82  bool is_init;


83  flair::gui::DoubleSpinBox *Q_angle,*Q_gyro,*R_angle;


84  flair::core::Euler euler;


85  };


86 


87  #endif // AHRSKALMAN_IMPL_H

