EKF.h

00001 //------------------------------------------------------------------------------
00002 // Extended Kalman Filter for a sensor fusion (Gyroscope and accelataion sensor)
00003 //------------------------------------------------------------------------------
00004 #ifndef EKF_H
00005 #define EKF_H
00006 
00007 #include "mbed.h"
00008 #include "math.h"
00009 #include "Eigen/Core.h"
00010 #include "Eigen/Geometry.h"
00011 using namespace Eigen;
00012 
00013 //----Variables
00014 float wx, wy, wz;
00015 float s_pre_a, s_pre_b, c_pre_a, c_pre_b;
00016 float preEst_a, preEst_b;
00017 float s2_pre_a, c2_pre_a;
00018 float af1_a, af1_b, af2_a, af2_b;
00019 float pre_a = 0.0;
00020 float pre_b = 0.0;
00021 float estAlpha, estBeta;
00022 float b = 1.0f;
00023 //--For check
00024 float xhat0, xhat1;
00025 float af00, af01, af10, af11;
00026 float P00, P01, P10, P11;
00027 float KG00, KG01, KG10, KG11;
00028 float eye00, eye01, eye10, eye11;
00029 
00030 //----Vector
00031 Vector2f y;
00032 Vector2f h;
00033 Vector2f xhat;
00034 Vector2f xhat_new;
00035 
00036 //----Matrix
00037 Matrix2f eye = Matrix2f::Identity();
00038 Matrix2f af;
00039 Matrix2f ah = eye;
00040 Matrix2f P = 1*eye;
00041 Matrix2f pre_P = 1*eye;
00042 Matrix2f P_new;
00043 Matrix2f KG_den;    // denominator of kalman gain
00044 Matrix2f KalmanGain;
00045 Matrix2f Q = 0.001*eye; // Covariance matrix
00046 Matrix2f R = 0.001*eye;
00047 
00048 class EKF
00049 {
00050   protected:
00051     
00052   public:
00053   void ExtendedKalmanFilterUpdate(float th_ax, float th_ay, float pre_wx, float pre_wy, float pre_wz)
00054   {
00055     //----Prediction step
00056     //--Previous estimated state
00057     s_pre_a = sin(pre_a);
00058     c_pre_a = cos(pre_a);
00059     s_pre_b = sin(pre_b);
00060     c_pre_b = cos(pre_b);
00061     
00062     // PreEst alpha, beta
00063     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));
00064     xhat0 = xhat(0);
00065     xhat(1) = pre_b + delt_t*(pre_wy*c_pre_a*c_pre_b + pre_wz*s_pre_a*s_pre_b);
00066     xhat1 = xhat(1);
00067     
00068     //--Linearized system
00069     s2_pre_a = sin(2.0f*pre_a);
00070     c2_pre_a = cos(2.0f*pre_a);
00071     // af1_a, af1_b, af2_a, af2_b
00072     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);
00073     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);
00074     af(1,0) = delt_t*(-pre_wy*s_pre_a*c_pre_b + pre_wz*c_pre_a*s_pre_b);
00075     af(1,1) = 1.0f + delt_t*(-pre_wy*(c_pre_a*s_pre_b) + pre_wz*(s_pre_a*c_pre_b));
00076     
00077     //--Previous error covariance matrix
00078     P = af*pre_P*af.transpose() + b*Q*b;
00079     
00080     //----Filtering step
00081     //--Kalman gain calulation
00082     KG_den = ah.transpose()*P*ah + R;
00083     KalmanGain = (P*ah)*KG_den.inverse();
00084     
00085     /*
00086     KG00 = KalmanGain(0,0);
00087     KG01 = KalmanGain(0,1);
00088     KG10 = KalmanGain(1,0);
00089     KG11 = KalmanGain(1,1);
00090     */
00091     
00092     //--New Estimated state
00093     h(0) = xhat0;
00094     h(1) = xhat1;
00095     y(0) = th_ax;
00096     y(1) = th_ay;
00097     xhat_new = xhat + KalmanGain*(y - h);
00098     estAlpha = xhat_new(0);
00099     estBeta = xhat_new(1);
00100     
00101     //--New covariance matrix
00102     P_new = (eye - KalmanGain*ah.transpose())*P;
00103     
00104     //--Set the current value as previous value
00105     pre_wx = wx;
00106     pre_wy = wy;
00107     pre_wz = wz;
00108     pre_th_ax = th_ax;
00109     pre_th_ay = th_ay;
00110     pre_P = P_new;
00111     pre_a = estAlpha;
00112     pre_b = estBeta;    
00113   }
00114 };
00115 #endif