Chris LU / Mbed 2 deprecated Self_Riding_Bicycle

2018/06/08

Dependencies:   LSM9DS0 mbed

SensorFusion.cpp@0:bf9bf4b7625f, 2018-06-08 (annotated)

Committer:
cpul5338
Date:
Fri Jun 08 14:11:49 2018 +0000
Revision:
0:bf9bf4b7625f
2018/06/08

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cpul5338 0:bf9bf4b7625f 1 #include "SensorFusion.h"
cpul5338 0:bf9bf4b7625f 2 #include "SystemConstant.h"
cpul5338 0:bf9bf4b7625f 3 #include "math.h"
cpul5338 0:bf9bf4b7625f 4
cpul5338 0:bf9bf4b7625f 5 ///Gyro and Acce data
cpul5338 0:bf9bf4b7625f 6 float axm = 0.0f;
cpul5338 0:bf9bf4b7625f 7 float aym = 0.0f;
cpul5338 0:bf9bf4b7625f 8 float azm = 0.0f;
cpul5338 0:bf9bf4b7625f 9 float u1 = 0.0f;
cpul5338 0:bf9bf4b7625f 10 float u2 = 0.0f;
cpul5338 0:bf9bf4b7625f 11 float u3 = 0.0f;
cpul5338 0:bf9bf4b7625f 12 float mx = 0.0f;
cpul5338 0:bf9bf4b7625f 13 float my = 0.0f;
cpul5338 0:bf9bf4b7625f 14 float mz = 0.0f;
cpul5338 0:bf9bf4b7625f 15 float Ac[3] = {0.0f, 0.0f, 0.0f};
cpul5338 0:bf9bf4b7625f 16
cpul5338 0:bf9bf4b7625f 17 float axm_f = 0.0f;
cpul5338 0:bf9bf4b7625f 18 float axm_f_old = 0.0f;
cpul5338 0:bf9bf4b7625f 19 float u3aym_f = 0.0f;
cpul5338 0:bf9bf4b7625f 20 float u3aym_f_old = 0.0f;
cpul5338 0:bf9bf4b7625f 21 float u2azm_f = 0.0f;
cpul5338 0:bf9bf4b7625f 22 float u2azm_f_old = 0.0f;
cpul5338 0:bf9bf4b7625f 23
cpul5338 0:bf9bf4b7625f 24 float aym_f = 0.0f;
cpul5338 0:bf9bf4b7625f 25 float aym_f_old = 0.0f;
cpul5338 0:bf9bf4b7625f 26 float u3axm_f = 0.0f;
cpul5338 0:bf9bf4b7625f 27 float u3axm_f_old = 0.0f;
cpul5338 0:bf9bf4b7625f 28 float u1azm_f = 0.0f;
cpul5338 0:bf9bf4b7625f 29 float u1azm_f_old = 0.0f;
cpul5338 0:bf9bf4b7625f 30
cpul5338 0:bf9bf4b7625f 31 float azm_f = 0.0f;
cpul5338 0:bf9bf4b7625f 32 float azm_f_old = 0.0f;
cpul5338 0:bf9bf4b7625f 33 float u2axm_f = 0.0f;
cpul5338 0:bf9bf4b7625f 34 float u2axm_f_old = 0.0f;
cpul5338 0:bf9bf4b7625f 35 float u1aym_f = 0.0f;
cpul5338 0:bf9bf4b7625f 36 float u1aym_f_old = 0.0f;
cpul5338 0:bf9bf4b7625f 37
cpul5338 0:bf9bf4b7625f 38 float x1_hat = 0.0f;
cpul5338 0:bf9bf4b7625f 39 float x2_hat = 0.0f;
cpul5338 0:bf9bf4b7625f 40 float x3_hat = 0.0f;
cpul5338 0:bf9bf4b7625f 41 float sinroll = 0.0f;
cpul5338 0:bf9bf4b7625f 42 float cosroll = 0.0f;
cpul5338 0:bf9bf4b7625f 43 float roll_angle = 0.0f;
cpul5338 0:bf9bf4b7625f 44 float roll_angle_old = 0.0f;
cpul5338 0:bf9bf4b7625f 45 float droll_angle= 0.0f;
cpul5338 0:bf9bf4b7625f 46 float droll_angle_old= 0.0f;
cpul5338 0:bf9bf4b7625f 47 float sinpitch = 0.0f;
cpul5338 0:bf9bf4b7625f 48 float cospitch = 0.0f;
cpul5338 0:bf9bf4b7625f 49 float yaw_ref = 0.0f;
cpul5338 0:bf9bf4b7625f 50 float yaw_angle = 0.0f;
cpul5338 0:bf9bf4b7625f 51 float yaw_angle_old = 0.0f;
cpul5338 0:bf9bf4b7625f 52 float dyaw_angle = 0.0f;
cpul5338 0:bf9bf4b7625f 53 float dyaw_angle_old = 0.0f;
cpul5338 0:bf9bf4b7625f 54 // ***************************************************************************************************************************************************************************
cpul5338 0:bf9bf4b7625f 55 // Low-Pass Filter
cpul5338 0:bf9bf4b7625f 56 float lpf(float input,float input_old,float frequency)
cpul5338 0:bf9bf4b7625f 57 {
cpul5338 0:bf9bf4b7625f 58 float output = 0;
cpul5338 0:bf9bf4b7625f 59
cpul5338 0:bf9bf4b7625f 60 //y[n] = a*y[n-1] + (1 - a)*x[n]
cpul5338 0:bf9bf4b7625f 61 output = input*frequency*sample_time+input_old*(1-frequency*sample_time);
cpul5338 0:bf9bf4b7625f 62
cpul5338 0:bf9bf4b7625f 63 return output;
cpul5338 0:bf9bf4b7625f 64 }// Low-Pass Filter
cpul5338 0:bf9bf4b7625f 65
cpul5338 0:bf9bf4b7625f 66 // ***************************************************************************************************************************************************************************
cpul5338 0:bf9bf4b7625f 67 // x2_hat_estimat with finding roll angle
cpul5338 0:bf9bf4b7625f 68 void roll_fusion(float a_xm,float a_ym,float a_zm,float u_3,float u_1,float alpha)
cpul5338 0:bf9bf4b7625f 69 {
cpul5338 0:bf9bf4b7625f 70 // x2_hat estimation
cpul5338 0:bf9bf4b7625f 71 aym_f = lpf(a_ym,aym_f_old,alpha);
cpul5338 0:bf9bf4b7625f 72 aym_f_old = aym_f;
cpul5338 0:bf9bf4b7625f 73 u3axm_f = lpf(u_3*a_xm,u3axm_f_old,alpha); // w3axm
cpul5338 0:bf9bf4b7625f 74 u3axm_f_old = u3axm_f;
cpul5338 0:bf9bf4b7625f 75 u1azm_f = lpf(u_1*a_zm,u1azm_f_old,alpha); // w1azm
cpul5338 0:bf9bf4b7625f 76 u1azm_f_old = u1azm_f;
cpul5338 0:bf9bf4b7625f 77
cpul5338 0:bf9bf4b7625f 78 x2_hat = -u3axm_f/alpha + aym_f + u1azm_f/alpha; //THe Low-Pass Filter(alpha/(s+alpha)) have already been done
cpul5338 0:bf9bf4b7625f 79
cpul5338 0:bf9bf4b7625f 80 // Finding the roll angle and giving the limit for it
cpul5338 0:bf9bf4b7625f 81 sinroll = x2_hat*(-0.1020); // This is for finding the roll angle, because x2_hat = -gsin(roll_angle). This is from paper Observer-Based Sensor Fusion.
cpul5338 0:bf9bf4b7625f 82 if(sinroll >= 1.0f)
cpul5338 0:bf9bf4b7625f 83 {
cpul5338 0:bf9bf4b7625f 84 sinroll = 1.0;
cpul5338 0:bf9bf4b7625f 85 cosroll = 0.0;
cpul5338 0:bf9bf4b7625f 86 }
cpul5338 0:bf9bf4b7625f 87 else if(sinroll <= -1.0f)
cpul5338 0:bf9bf4b7625f 88 {
cpul5338 0:bf9bf4b7625f 89 sinroll = -1.0;
cpul5338 0:bf9bf4b7625f 90 cosroll = 0.0;
cpul5338 0:bf9bf4b7625f 91 }
cpul5338 0:bf9bf4b7625f 92 else cosroll = sqrt(1-(sinroll*sinroll));
cpul5338 0:bf9bf4b7625f 93 roll_angle = asin(sinroll);///By 312 Rotation
cpul5338 0:bf9bf4b7625f 94 // roll_angle = lpf(roll_angle, roll_angle_old, 3);
cpul5338 0:bf9bf4b7625f 95 // roll_angle_old = roll_angle;
cpul5338 0:bf9bf4b7625f 96 }// roll_fusion
cpul5338 0:bf9bf4b7625f 97
cpul5338 0:bf9bf4b7625f 98 // ***************************************************************************************************************************************************************************
cpul5338 0:bf9bf4b7625f 99 // absolute
cpul5338 0:bf9bf4b7625f 100 float absolute(float value)
cpul5338 0:bf9bf4b7625f 101 {
cpul5338 0:bf9bf4b7625f 102 if(value < 0)return -value;
cpul5338 0:bf9bf4b7625f 103 else return value;
cpul5338 0:bf9bf4b7625f 104 }// absolute
cpul5338 0:bf9bf4b7625f 105
cpul5338 0:bf9bf4b7625f 106 // ***************************************************************************************************************************************************************************
cpul5338 0:bf9bf4b7625f 107 // Reset_data
cpul5338 0:bf9bf4b7625f 108 void Reset_data(void)
cpul5338 0:bf9bf4b7625f 109 {
cpul5338 0:bf9bf4b7625f 110 axm_f = axm_f_old = u3aym_f = u3aym_f_old = u2azm_f = u2azm_f_old = 0.0;
cpul5338 0:bf9bf4b7625f 111 aym_f = aym_f_old = u3axm_f = u3axm_f_old = u1azm_f = u1azm_f_old = 0.0;
cpul5338 0:bf9bf4b7625f 112 azm_f = azm_f_old = u2axm_f = u2axm_f_old = u1aym_f = u1aym_f_old = 0.0;
cpul5338 0:bf9bf4b7625f 113 }// Reset_data