File content as of revision 3:e7be5e23013d:

//------------------------------------------------------------------------------
// Extended Kalman Filter for a sensor fusion (Gyroscope and accelataion sensor)
//------------------------------------------------------------------------------
#ifndef EKF_H
#define EKF_H

#include "mbed.h"
#include "math.h"
#include "Eigen/Core.h"
#include "Eigen/Geometry.h"
using namespace Eigen;

//----Variables
float wx, wy, wz;
float s_pre_a, s_pre_b, c_pre_a, c_pre_b;
float preEst_a, preEst_b;
float s2_pre_a, c2_pre_a;
float af1_a, af1_b, af2_a, af2_b;
float pre_a = 0.0;
float pre_b = 0.0;
float estAlpha, estBeta;
float b = 1.0f;
//--For check
float xhat0, xhat1;
float af00, af01, af10, af11;
float P00, P01, P10, P11;
float KG00, KG01, KG10, KG11;
float eye00, eye01, eye10, eye11;

//----Vector
Vector2f y;
Vector2f h;
Vector2f xhat;
Vector2f xhat_new;

//----Matrix
Matrix2f eye = Matrix2f::Identity();
Matrix2f af;
Matrix2f ah = eye;
Matrix2f P = 1*eye;
Matrix2f pre_P = 1*eye;
Matrix2f P_new;
Matrix2f KG_den;    // denominator of kalman gain
Matrix2f KalmanGain;
Matrix2f Q = 0.001*eye; // Covariance matrix
Matrix2f R = 0.001*eye;

class EKF
{
  protected:
    
  public:
  void ExtendedKalmanFilterUpdate(float th_ax, float th_ay, float pre_wx, float pre_wy, float pre_wz)
  {
    //----Prediction step
    //--Previous estimated state
    s_pre_a = sin(pre_a);
    c_pre_a = cos(pre_a);
    s_pre_b = sin(pre_b);
    c_pre_b = cos(pre_b);
    
    // PreEst alpha, beta
    xhat(0) = pre_a + delt_t*(pre_wx*(c_pre_a*c_pre_a*c_pre_b + s_pre_a*s_pre_a*s_pre_b) + pre_wy*(s_pre_a*s_pre_b) - pre_wz*(c_pre_a*c_pre_b));
    xhat0 = xhat(0);
    xhat(1) = pre_b + delt_t*(pre_wy*c_pre_a*c_pre_b + pre_wz*s_pre_a*s_pre_b);
    xhat1 = xhat(1);
    
    //--Linearized system
    s2_pre_a = sin(2.0f*pre_a);
    c2_pre_a = cos(2.0f*pre_a);
    // af1_a, af1_b, af2_a, af2_b
    af(0,0) = 1.0f + delt_t*(pre_wx*(-s2_pre_a*c_pre_b + s2_pre_a*s_pre_b) + pre_wy*c_pre_a*s_pre_b);
    af(0,1) = delt_t*(-pre_wx*(c_pre_a*c_pre_a*s_pre_b + s_pre_b*s_pre_a*c_pre_b) + pre_wy*s_pre_a*c_pre_b);
    af(1,0) = delt_t*(-pre_wy*s_pre_a*c_pre_b + pre_wz*c_pre_a*s_pre_b);
    af(1,1) = 1.0f + delt_t*(-pre_wy*(c_pre_a*s_pre_b) + pre_wz*(s_pre_a*c_pre_b));
    
    //--Previous error covariance matrix
    P = af*pre_P*af.transpose() + b*Q*b;
    
    //----Filtering step
    //--Kalman gain calulation
    KG_den = ah.transpose()*P*ah + R;
    KalmanGain = (P*ah)*KG_den.inverse();
    
    /*
    KG00 = KalmanGain(0,0);
    KG01 = KalmanGain(0,1);
    KG10 = KalmanGain(1,0);
    KG11 = KalmanGain(1,1);
    */
    
    //--New Estimated state
    h(0) = xhat0;
    h(1) = xhat1;
    y(0) = th_ax;
    y(1) = th_ay;
    xhat_new = xhat + KalmanGain*(y - h);
    estAlpha = xhat_new(0);
    estBeta = xhat_new(1);
    
    //--New covariance matrix
    P_new = (eye - KalmanGain*ah.transpose())*P;
    
    //--Set the current value as previous value
    pre_wx = wx;
    pre_wy = wy;
    pre_wz = wz;
    pre_th_ax = th_ax;
    pre_th_ay = th_ay;
    pre_P = P_new;
    pre_a = estAlpha;
    pre_b = estBeta;    
  }
};
#endif