Diff: EKF.h

Revision:

3:dc4292a7c440

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/EKF.h	Mon Jan 14 18:58:46 2019 +0000
@@ -0,0 +1,115 @@
+//------------------------------------------------------------------------------
+// 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