Akira Heya / Mbed 2 deprecated MPU9250_I2C_test

test

Dependencies:   mbed

MPU9250 I2C通信 注意：プルアップ抵抗を入れるのを忘れないように

EKF.h@3:e7be5e23013d, 2022-07-25 (annotated)

Committer:
AkiraHeya
Date:
Mon Jul 25 05:54:41 2022 +0000
Revision:
3:e7be5e23013d
IMU-MPU9250-TestCode

Who changed what in which revision?

UserRevisionLine numberNew contents of line
AkiraHeya 3:e7be5e23013d 1 //------------------------------------------------------------------------------
AkiraHeya 3:e7be5e23013d 2 // Extended Kalman Filter for a sensor fusion (Gyroscope and accelataion sensor)
AkiraHeya 3:e7be5e23013d 3 //------------------------------------------------------------------------------
AkiraHeya 3:e7be5e23013d 4 #ifndef EKF_H
AkiraHeya 3:e7be5e23013d 5 #define EKF_H
AkiraHeya 3:e7be5e23013d 6
AkiraHeya 3:e7be5e23013d 7 #include "mbed.h"
AkiraHeya 3:e7be5e23013d 8 #include "math.h"
AkiraHeya 3:e7be5e23013d 9 #include "Eigen/Core.h"
AkiraHeya 3:e7be5e23013d 10 #include "Eigen/Geometry.h"
AkiraHeya 3:e7be5e23013d 11 using namespace Eigen;
AkiraHeya 3:e7be5e23013d 12
AkiraHeya 3:e7be5e23013d 13 //----Variables
AkiraHeya 3:e7be5e23013d 14 float wx, wy, wz;
AkiraHeya 3:e7be5e23013d 15 float s_pre_a, s_pre_b, c_pre_a, c_pre_b;
AkiraHeya 3:e7be5e23013d 16 float preEst_a, preEst_b;
AkiraHeya 3:e7be5e23013d 17 float s2_pre_a, c2_pre_a;
AkiraHeya 3:e7be5e23013d 18 float af1_a, af1_b, af2_a, af2_b;
AkiraHeya 3:e7be5e23013d 19 float pre_a = 0.0;
AkiraHeya 3:e7be5e23013d 20 float pre_b = 0.0;
AkiraHeya 3:e7be5e23013d 21 float estAlpha, estBeta;
AkiraHeya 3:e7be5e23013d 22 float b = 1.0f;
AkiraHeya 3:e7be5e23013d 23 //--For check
AkiraHeya 3:e7be5e23013d 24 float xhat0, xhat1;
AkiraHeya 3:e7be5e23013d 25 float af00, af01, af10, af11;
AkiraHeya 3:e7be5e23013d 26 float P00, P01, P10, P11;
AkiraHeya 3:e7be5e23013d 27 float KG00, KG01, KG10, KG11;
AkiraHeya 3:e7be5e23013d 28 float eye00, eye01, eye10, eye11;
AkiraHeya 3:e7be5e23013d 29
AkiraHeya 3:e7be5e23013d 30 //----Vector
AkiraHeya 3:e7be5e23013d 31 Vector2f y;
AkiraHeya 3:e7be5e23013d 32 Vector2f h;
AkiraHeya 3:e7be5e23013d 33 Vector2f xhat;
AkiraHeya 3:e7be5e23013d 34 Vector2f xhat_new;
AkiraHeya 3:e7be5e23013d 35
AkiraHeya 3:e7be5e23013d 36 //----Matrix
AkiraHeya 3:e7be5e23013d 37 Matrix2f eye = Matrix2f::Identity();
AkiraHeya 3:e7be5e23013d 38 Matrix2f af;
AkiraHeya 3:e7be5e23013d 39 Matrix2f ah = eye;
AkiraHeya 3:e7be5e23013d 40 Matrix2f P = 1*eye;
AkiraHeya 3:e7be5e23013d 41 Matrix2f pre_P = 1*eye;
AkiraHeya 3:e7be5e23013d 42 Matrix2f P_new;
AkiraHeya 3:e7be5e23013d 43 Matrix2f KG_den; // denominator of kalman gain
AkiraHeya 3:e7be5e23013d 44 Matrix2f KalmanGain;
AkiraHeya 3:e7be5e23013d 45 Matrix2f Q = 0.001*eye; // Covariance matrix
AkiraHeya 3:e7be5e23013d 46 Matrix2f R = 0.001*eye;
AkiraHeya 3:e7be5e23013d 47
AkiraHeya 3:e7be5e23013d 48 class EKF
AkiraHeya 3:e7be5e23013d 49 {
AkiraHeya 3:e7be5e23013d 50 protected:
AkiraHeya 3:e7be5e23013d 51
AkiraHeya 3:e7be5e23013d 52 public:
AkiraHeya 3:e7be5e23013d 53 void ExtendedKalmanFilterUpdate(float th_ax, float th_ay, float pre_wx, float pre_wy, float pre_wz)
AkiraHeya 3:e7be5e23013d 54 {
AkiraHeya 3:e7be5e23013d 55 //----Prediction step
AkiraHeya 3:e7be5e23013d 56 //--Previous estimated state
AkiraHeya 3:e7be5e23013d 57 s_pre_a = sin(pre_a);
AkiraHeya 3:e7be5e23013d 58 c_pre_a = cos(pre_a);
AkiraHeya 3:e7be5e23013d 59 s_pre_b = sin(pre_b);
AkiraHeya 3:e7be5e23013d 60 c_pre_b = cos(pre_b);
AkiraHeya 3:e7be5e23013d 61
AkiraHeya 3:e7be5e23013d 62 // PreEst alpha, beta
AkiraHeya 3:e7be5e23013d 63 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));
AkiraHeya 3:e7be5e23013d 64 xhat0 = xhat(0);
AkiraHeya 3:e7be5e23013d 65 xhat(1) = pre_b + delt_t*(pre_wy*c_pre_a*c_pre_b + pre_wz*s_pre_a*s_pre_b);
AkiraHeya 3:e7be5e23013d 66 xhat1 = xhat(1);
AkiraHeya 3:e7be5e23013d 67
AkiraHeya 3:e7be5e23013d 68 //--Linearized system
AkiraHeya 3:e7be5e23013d 69 s2_pre_a = sin(2.0f*pre_a);
AkiraHeya 3:e7be5e23013d 70 c2_pre_a = cos(2.0f*pre_a);
AkiraHeya 3:e7be5e23013d 71 // af1_a, af1_b, af2_a, af2_b
AkiraHeya 3:e7be5e23013d 72 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);
AkiraHeya 3:e7be5e23013d 73 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);
AkiraHeya 3:e7be5e23013d 74 af(1,0) = delt_t*(-pre_wy*s_pre_a*c_pre_b + pre_wz*c_pre_a*s_pre_b);
AkiraHeya 3:e7be5e23013d 75 af(1,1) = 1.0f + delt_t*(-pre_wy*(c_pre_a*s_pre_b) + pre_wz*(s_pre_a*c_pre_b));
AkiraHeya 3:e7be5e23013d 76
AkiraHeya 3:e7be5e23013d 77 //--Previous error covariance matrix
AkiraHeya 3:e7be5e23013d 78 P = af*pre_P*af.transpose() + b*Q*b;
AkiraHeya 3:e7be5e23013d 79
AkiraHeya 3:e7be5e23013d 80 //----Filtering step
AkiraHeya 3:e7be5e23013d 81 //--Kalman gain calulation
AkiraHeya 3:e7be5e23013d 82 KG_den = ah.transpose()*P*ah + R;
AkiraHeya 3:e7be5e23013d 83 KalmanGain = (P*ah)*KG_den.inverse();
AkiraHeya 3:e7be5e23013d 84
AkiraHeya 3:e7be5e23013d 85 /*
AkiraHeya 3:e7be5e23013d 86 KG00 = KalmanGain(0,0);
AkiraHeya 3:e7be5e23013d 87 KG01 = KalmanGain(0,1);
AkiraHeya 3:e7be5e23013d 88 KG10 = KalmanGain(1,0);
AkiraHeya 3:e7be5e23013d 89 KG11 = KalmanGain(1,1);
AkiraHeya 3:e7be5e23013d 90 */
AkiraHeya 3:e7be5e23013d 91
AkiraHeya 3:e7be5e23013d 92 //--New Estimated state
AkiraHeya 3:e7be5e23013d 93 h(0) = xhat0;
AkiraHeya 3:e7be5e23013d 94 h(1) = xhat1;
AkiraHeya 3:e7be5e23013d 95 y(0) = th_ax;
AkiraHeya 3:e7be5e23013d 96 y(1) = th_ay;
AkiraHeya 3:e7be5e23013d 97 xhat_new = xhat + KalmanGain*(y - h);
AkiraHeya 3:e7be5e23013d 98 estAlpha = xhat_new(0);
AkiraHeya 3:e7be5e23013d 99 estBeta = xhat_new(1);
AkiraHeya 3:e7be5e23013d 100
AkiraHeya 3:e7be5e23013d 101 //--New covariance matrix
AkiraHeya 3:e7be5e23013d 102 P_new = (eye - KalmanGain*ah.transpose())*P;
AkiraHeya 3:e7be5e23013d 103
AkiraHeya 3:e7be5e23013d 104 //--Set the current value as previous value
AkiraHeya 3:e7be5e23013d 105 pre_wx = wx;
AkiraHeya 3:e7be5e23013d 106 pre_wy = wy;
AkiraHeya 3:e7be5e23013d 107 pre_wz = wz;
AkiraHeya 3:e7be5e23013d 108 pre_th_ax = th_ax;
AkiraHeya 3:e7be5e23013d 109 pre_th_ay = th_ay;
AkiraHeya 3:e7be5e23013d 110 pre_P = P_new;
AkiraHeya 3:e7be5e23013d 111 pre_a = estAlpha;
AkiraHeya 3:e7be5e23013d 112 pre_b = estBeta;
AkiraHeya 3:e7be5e23013d 113 }
AkiraHeya 3:e7be5e23013d 114 };
AkiraHeya 3:e7be5e23013d 115 #endif