SensorFusion.cpp

00001 #include "SensorFusion.h"
00002 #include "SystemConstant.h"
00003 #include "math.h"
00004 
00005 ///Gyro and Acce data
00006 float axm = 0.0f;
00007 float aym = 0.0f;
00008 float azm = 0.0f;
00009 float u1 = 0.0f;
00010 float u2 = 0.0f;
00011 float u3 = 0.0f;
00012 float mx = 0.0f;
00013 float my = 0.0f;
00014 float mz = 0.0f;
00015 float Ac[3] = {0.0f, 0.0f, 0.0f};
00016 
00017 float axm_f = 0.0f;
00018 float axm_f_old = 0.0f;
00019 float u3aym_f = 0.0f;
00020 float u3aym_f_old = 0.0f;
00021 float u2azm_f = 0.0f;
00022 float u2azm_f_old = 0.0f;
00023 
00024 float aym_f = 0.0f;
00025 float aym_f_old = 0.0f;
00026 float u3axm_f = 0.0f;
00027 float u3axm_f_old = 0.0f;
00028 float u1azm_f = 0.0f;
00029 float u1azm_f_old = 0.0f;
00030 
00031 float azm_f = 0.0f;
00032 float azm_f_old = 0.0f;
00033 float u2axm_f = 0.0f;
00034 float u2axm_f_old = 0.0f;
00035 float u1aym_f = 0.0f;
00036 float u1aym_f_old = 0.0f;
00037 
00038 float x1_hat = 0.0f;
00039 float x2_hat = 0.0f;
00040 float x3_hat = 0.0f;
00041 float sinroll = 0.0f;
00042 float cosroll = 0.0f;
00043 float roll_angle = 0.0f;
00044 float roll_angle_old = 0.0f;
00045 float droll_angle= 0.0f;
00046 float droll_angle_old= 0.0f;
00047 float sinpitch = 0.0f;
00048 float cospitch = 0.0f;
00049 float yaw_ref = 0.0f;
00050 float yaw_angle = 0.0f;
00051 float yaw_angle_old = 0.0f;
00052 float dyaw_angle = 0.0f;
00053 float dyaw_angle_old = 0.0f;
00054 // ***************************************************************************************************************************************************************************
00055 //                                                                              Low-Pass Filter
00056 float lpf(float input,float input_old,float frequency)
00057 {
00058     float output = 0;
00059     
00060     //y[n] = a*y[n-1] + (1 - a)*x[n]
00061     output = input*frequency*sample_time+input_old*(1-frequency*sample_time);
00062     
00063     return output;
00064 }// Low-Pass Filter
00065 
00066 // ***************************************************************************************************************************************************************************
00067 //                                                                              x2_hat_estimat with finding roll angle
00068 void roll_fusion(float a_xm,float a_ym,float a_zm,float u_3,float u_1,float alpha)
00069 {
00070     // x2_hat estimation
00071     aym_f = lpf(a_ym,aym_f_old,alpha);
00072     aym_f_old = aym_f;
00073     u3axm_f = lpf(u_3*a_xm,u3axm_f_old,alpha); // w3axm
00074     u3axm_f_old = u3axm_f;
00075     u1azm_f = lpf(u_1*a_zm,u1azm_f_old,alpha); // w1azm
00076     u1azm_f_old = u1azm_f;
00077     
00078     x2_hat = -u3axm_f/alpha + aym_f + u1azm_f/alpha; //THe Low-Pass Filter(alpha/(s+alpha)) have already been done  
00079       
00080    // Finding the roll angle and giving the limit for it
00081     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.
00082     if(sinroll >= 1.0f)
00083     {
00084         sinroll = 1.0;
00085         cosroll = 0.0;
00086     }
00087     else if(sinroll <= -1.0f)
00088     {
00089         sinroll = -1.0;
00090         cosroll = 0.0;
00091     }
00092     else cosroll = sqrt(1-(sinroll*sinroll));
00093     roll_angle = asin(sinroll);///By 312 Rotation
00094 //    roll_angle = lpf(roll_angle, roll_angle_old, 3);
00095 //    roll_angle_old = roll_angle;
00096 }// roll_fusion
00097 
00098 // ***************************************************************************************************************************************************************************
00099 //                                                                              absolute
00100 float absolute(float value)
00101 {
00102     if(value < 0)return -value;
00103     else return value;
00104 }// absolute
00105 
00106 // ***************************************************************************************************************************************************************************
00107 //                                                                              Reset_data
00108 void Reset_data(void)
00109 {
00110     axm_f = axm_f_old = u3aym_f = u3aym_f_old = u2azm_f = u2azm_f_old = 0.0;
00111     aym_f = aym_f_old = u3axm_f = u3axm_f_old = u1azm_f = u1azm_f_old = 0.0;
00112     azm_f = azm_f_old = u2axm_f = u2axm_f_old = u1aym_f = u1aym_f_old = 0.0;
00113 }// Reset_data