openXsensor/KalmanFilter.h

   1 #ifndef KALMAN_FILTER_H_
   2 #define KALMAN_FILTER_H_
   3
   4 #include "oXs_config_basic.h"
   5 #include "oXs_config_advanced.h"
   6 #include "oXs_config_macros.h"
   7
   8 #ifdef DEBUG
   9 //  #define DEBUG_KALMAN_TIME     // this print (in openXsensor.ino) the time to calculate kalman filter.
  10 #endif
  11
  12 class KalmanFilter {
  13
  14 public :
  15
  16 KalmanFilter() ;
  17 //void Configure( float zInitial ); // is not used anymore because all data are initilalise in KalmanFilter.h or with #define
  18 void Update(float z, float a,  float* pZ, float* pV);
  19 //float getPaa_() ;
  20
  21 private :
  22
  23 // State being tracked
  24         float z_;  // position
  25         float v_;  // velocity
  26         float aBias_;  // acceleration
  27
  28 // 3x3 State Covariance matrix
  29         float Pzz_ ;
  30         float Pzv_;
  31         float Pza_;
  32         float Pvz_;
  33         float Pvv_ ;
  34         float Pva_;
  35         float Paz_;
  36         float Pav_;
  37         float Paa_ ;
  38
  39 // those float have been replaced by #define to save flash memory
  40 //  float zAccelBiasVariance_; // assumed fixed.
  41 //      float zAccelVariance_;  // dynamic acceleration variance
  42 //      float zVariance_; //  z measurement noise variance fixed
  43
  44 };
  45
  46 #endif