openXsensor/KalmanFilter.cpp

   1 #include "Arduino.h"
   2 // #include "common.h"
   3 #include <math.h>
   4 #include "KalmanFilter.h"
   5
   6 // Tracks the position z and velocity v of an object moving in a straight line,
   7 // (here assumed to be vertical) that is perturbed by random accelerations.
   8 // sensor measurement of z is assumed to have constant measurement noise
   9 // variance zVariance,
  10 // This can be calculated offline for the specific sensor, and is supplied
  11 // as an initialization parameter.
  12
  13 #ifdef DEBUG_KALMAN_TIME
  14 extern int delayKalman[5] ;
  15 #endif
  16
  17 KalmanFilter::KalmanFilter() { // constructor initialize 3 parameters
  18   Pzz_ = Pvv_ = 1.0f ;
  19   Paa_ = 100000.0f;
  20 }
  21
  22 /* // model of a function to extract data from Kalman filter
  23 float KalmanFilter::getPaa_() {
  24   return Paa_ ;
  25 }
  26 */
  27 /*
  28 //void KalmanFilter::Configure(float zVariance, float zAccelVariance, float zAccelBiasVariance, float zInitial, float vInitial, float aBiasInitial) {
  29 void KalmanFilter::Configure( float zInitial) { // is not used anymore because all data are initilalise in KalmanFilter.h or with #define
  30 //      zAccelVariance_ = zAccelVariance;
  31 //  zAccelBiasVariance_ = zAccelBiasVariance;
  32 //      zVariance_ = zVariance;
  33
  34 //      z_ = zInitial;
  35 //      v_ =  0.0f; // is automatically initialise to 0 ; this save some flash memory
  36 //  aBias_ = 0.0f;
  37
  38 //      Pzz_ = 1.0f;
  39 //      Pzv_ = 0.0f;
  40 //      Pza_ = 0.0f;
  41
  42 //      Pvz_ = 0.0f;
  43 //      Pvv_ = 1.0f;
  44 //      Pva_ = 0.0f;
  45
  46 //      Paz_ = 0.0f;
  47 //      Pav_ = 0.0;
  48 //      Paa_ = 100000.0f;
  49
  50         }
  51 */
  52
  53 // Updates state given a sensor measurement of z, acceleration a,
  54 // and the time in seconds dt since the last measurement.
  55 // 19uS on Navspark @81.84MHz
  56 //void KalmanFilter::Update(float z, float a, float dt, float* pZ, float* pV) {
  57 void KalmanFilter::Update(float z, float a,  float* pZ, float* pV) {               // Update takes about 750 usec when running at 16 mHz
  58
  59 #define Z_VARIANCE        50.0f // initially 500
  60 #define ZACCEL_VARIANCE     0.1f  // initially 1
  61 #define ZACCELBIAS_VARIANCE 1.0f
  62
  63 #define DT 0.02f                          // time interval = 20 msec
  64 #define DT2DIV2  (DT * DT / 2.0f)
  65 #define DT3DIV2  (DT2DIV2 * DT)
  66 #define DT4DIV4  (DT2DIV2 * DT2DIV2)
  67
  68 #ifdef DEBUG_KALMAN_TIME
  69 unsigned long enterKalman = micros() ;
  70 #endif
  71
  72         // Predict state
  73     float accel = a - aBias_;
  74           v_ += accel * DT;
  75           z_ += v_ * DT;
  76
  77 //test by ms leaving zAccelVariance_ = 1
  78 /*
  79     zAccelVariance_ = fabs(accel)/50.0f;
  80     if (zAccelVariance_ > 50.0f)  {
  81         zAccelVariance_ = 50.0f;
  82     } else if (zAccelVariance_ < 1.0f) {
  83         zAccelVariance_ = 1.0f ;
  84     }
  85 //    CLAMP(zAccelVariance_, 1.0f, 50.0f);
  86 */
  87     // Predict State Covariance matrix
  88           float t00;
  89           float t01,t02;
  90     float t10,t11,t12;
  91     float t20,t21,t22;
  92
  93 //    float dt2div2 = dt*dt/2.0f;
  94 //    float dt3div2 = dt2div2*dt;
  95 //    float dt4div4 = dt2div2*dt2div2;
  96
  97         t00 = Pzz_ + DT*Pvz_ - DT2DIV2*Paz_;
  98         t01 = Pzv_ + DT*Pvv_ - DT2DIV2*Pav_;
  99         t02 = Pza_ + DT*Pva_ - DT2DIV2*Paa_;
 100
 101         t10 = Pvz_ - DT*Paz_;
 102         t11 = Pvv_ - DT*Pav_;
 103         t12 = Pva_ - DT*Paa_;
 104
 105         t20 = Paz_;
 106         t21 = Pav_;
 107         t22 = Paa_;
 108
 109 #ifdef DEBUG_KALMAN_TIME
 110   delayKalman[0] =  micros() - enterKalman ;
 111 #endif
 112         Pzz_ = t00 + DT*t01 - DT2DIV2*t02;
 113 //  Pzz_ +=  dt*Pvz_ - dt2div2*Paz_ + dt*t01 - dt2div2*t02;
 114         Pzv_ = t01 - DT*t02;
 115         Pza_ = t02;
 116
 117         Pvz_ = t10 + DT*t11 - DT2DIV2*t12;
 118 //  Pvz_ +=  - dt*Paz_ + dt*t11 - dt2div2*t12;
 119         Pvv_ = t11 - DT*t12;
 120         Pva_ = t12;
 121
 122         Paz_ = t20 + DT*t21 - DT2DIV2*t22;
 123 //  Paz_ +=  dt*t21 - dt2div2*t22;
 124         Pav_ = t21 - DT*t22;
 125         Paa_ = t22;
 126
 127 #ifdef DEBUG_KALMAN_TIME
 128   delayKalman[1] =  micros() - enterKalman ;
 129 #endif
 130
 131     Pzz_ += DT4DIV4*ZACCEL_VARIANCE;
 132     Pzv_ += DT3DIV2*ZACCEL_VARIANCE;
 133
 134     Pvz_ += DT3DIV2*ZACCEL_VARIANCE;
 135     Pvv_ += DT*DT*ZACCEL_VARIANCE;
 136
 137     Paa_ += ZACCELBIAS_VARIANCE;
 138
 139         // Error
 140         float innov = z - z_;
 141         float sInv = 1.0f / (Pzz_ + Z_VARIANCE);
 142
 143 #ifdef DEBUG_KALMAN_TIME
 144   delayKalman[2] =  micros() - enterKalman ;
 145 #endif
 146     // Kalman gains
 147         float kz = Pzz_ * sInv;
 148         float kv = Pvz_ * sInv;
 149         float ka = Paz_ * sInv;
 150
 151         // Update state
 152         z_ += kz * innov;
 153         v_ += kv * innov;
 154         aBias_ += ka * innov;
 155
 156         *pZ = z_;
 157         *pV = v_;
 158 #ifdef DEBUG_KALMAN_TIME
 159   delayKalman[3] =  micros() - enterKalman ;
 160 #endif
 161         // Update state covariance matrix
 162         Pzz_ -= kz * Pzz_;
 163         Pzv_ -= kz * Pzv_;
 164         Pza_ -= kz * Pza_;
 165
 166         Pvz_ -= kv * Pzz_;
 167         Pvv_ -= kv * Pzv_;
 168         Pva_ -= kv * Pza_;
 169
 170         Paz_ -= ka * Pzz_;
 171         Pav_ -= ka * Pzv_;
 172         Paa_ -= ka * Pza_;
 173 #ifdef DEBUG_KALMAN_TIME
 174   delayKalman[4] =  micros() - enterKalman ;
 175 #endif
 176         }