SENSOR_FUSION.cpp

00001 #include "SENSOR_FUSION.h"
00002 #include "math.h"
00003 
00004 int conv_init;
00005 
00006 float axm =0;
00007 float aym =0;
00008 float azm =0;
00009 float u1 = 0;
00010 float u2= 0;
00011 float u3= 0;
00012 float mx= 0;
00013 float my= 0;
00014 float mz= 0;
00015 
00016 float axm_f= 0;
00017 float axm_f_old= 0;
00018 float u3aym_f= 0;
00019 float u3aym_f_old= 0;
00020 float u2azm_f= 0;
00021 float u2azm_f_old= 0;
00022 
00023 float aym_f= 0;
00024 float aym_f_old= 0;
00025 float u3axm_f= 0;
00026 float u3axm_f_old= 0;
00027 float u1azm_f= 0;
00028 float u1azm_f_old= 0;
00029 
00030 float u2axm_f= 0;
00031 float u2axm_f_old= 0;
00032 float u1aym_f= 0;
00033 float u1aym_f_old= 0;
00034 float azm_f= 0;
00035 float azm_f_old= 0;
00036 
00037 
00038 float x1_hat = 0;
00039 float x2_hat= 0;
00040 float x3_hat= 0;
00041 float cospitch= 0;
00042 float pitch_angle= 0;
00043 float roll_angle = 0;
00044 
00045 float omega_phi= 0;
00046 
00047 float lpf(float in, float out_old, float alpha, float TS) {
00048     float out;
00049 
00050     out = (1. - alpha * TS) * out_old + alpha * TS*in;
00051     return out;
00052 }
00053 
00054 void pitch_fusion(float a_xm,float a_ym,float a_zm,float u_3,float u_2,float alpha,float TS)
00055 {
00056     axm_f = lpf(a_xm,axm_f_old,alpha,TS);
00057     axm_f_old = axm_f;
00058     u3aym_f = lpf(u_3*a_ym,u3aym_f_old,alpha, TS);
00059     u3aym_f_old = u3aym_f;
00060     u2azm_f = lpf(u_2*a_zm,u2azm_f_old,alpha, TS);
00061     u2azm_f_old = u2azm_f;    
00062     
00063     x1_hat = axm_f + u3aym_f/alpha - u2azm_f/alpha;
00064     cospitch = sqrt(1-(x1_hat*0.1020*x1_hat*0.1020));
00065     pitch_angle = asin(x1_hat*0.1020);
00066 }
00067 
00068 void roll_fusion(float a_xm,float a_ym,float a_zm,float u_3,float u_1,float alpha, float TS)
00069 {
00070     aym_f = lpf(a_ym,aym_f_old,alpha, TS);
00071     aym_f_old = aym_f;
00072     u3axm_f = lpf(u_3*a_xm,u3axm_f_old,alpha, TS);
00073     u3axm_f_old = u3axm_f;
00074     u1azm_f = lpf(u_1*a_zm,u1azm_f_old,alpha, TS);
00075     u1azm_f_old = u1azm_f;    
00076     
00077     x2_hat = -u3axm_f/alpha + aym_f + u1azm_f/alpha;
00078  
00079 }
00080 
00081 void x3_hat_estimat(float a_xm,float a_ym,float a_zm,float u_2,float u_1,float alpha, float TS)
00082 {
00083     u2axm_f = lpf(u_2*a_xm,u2axm_f_old,alpha, TS);
00084     u2axm_f_old = u2axm_f;
00085     u1aym_f = lpf(u_1*a_ym,u1aym_f_old,alpha, TS);
00086     u1aym_f_old = u1aym_f;
00087     azm_f = lpf(a_zm,azm_f_old,alpha, TS);
00088     azm_f_old = azm_f;    
00089     
00090     x3_hat = u2axm_f/alpha - u1aym_f/alpha + azm_f;
00091     
00092     roll_angle = asin(x3_hat*(-0.1012)/cospitch);
00093     omega_phi = u3;
00094     
00095 }