Chris LU / Mbed 2 deprecated Self_Riding_Bicycle_LQRgain

Self_Riding_Bicycle_LQRgain

Dependencies:   LSM9DS0_self_riding_bike_LQR mbed

SensorFusion.cpp@0:c2e43d17c8e4, 2018-08-20 (annotated)

Committer:
cpul5338
Date:
Mon Aug 20 13:21:31 2018 +0000
Revision:
0:c2e43d17c8e4
self_riding_bike_LQR

Who changed what in which revision?

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