Chun Feng Huang / ATTITUDE_ESTIMATION_WIPV

Attitude estimation using IMU (3-DoF ver.)

Fork of ATTITUDE_ESTIMATION by LDSC_Robotics_TAs

ATTITUDE_ESTIMATION.cpp@1:edc7ccfc5562, 2016-04-28 (annotated)

Committer:
benson516
Date:
Thu Apr 28 23:45:24 2016 +0000
Revision:
1:edc7ccfc5562
Parent:
0:8126c86bac2a
Child:
2:46d355d7abaa
Almost done, need to be tested

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benson516 0:8126c86bac2a 1 #include "ATTITUDE_ESTIMATION.h"
benson516 0:8126c86bac2a 2
benson516 1:edc7ccfc5562 3 ATTITUDE::ATTITUDE(float alpha_in, float Ts_in):alpha(alpha_in), Ts(Ts_in)
benson516 0:8126c86bac2a 4 {
benson516 1:edc7ccfc5562 5 //_unit_nk
benson516 1:edc7ccfc5562 6 //_zero3
benson516 1:edc7ccfc5562 7 float v1[3]={0.0,0.0,-1.0};
benson516 1:edc7ccfc5562 8 float v2[3]={0.0,0.0,0.0};
benson516 1:edc7ccfc5562 9 Set_vector3(_unit_nk,v1);
benson516 1:edc7ccfc5562 10 Set_vector3(_zero3,v2);
benson516 1:edc7ccfc5562 11
benson516 1:edc7ccfc5562 12
benson516 1:edc7ccfc5562 13 Set_vector3(EulerAngle,_zero3);
benson516 1:edc7ccfc5562 14 Set_vector3(ys,_zero3);
benson516 1:edc7ccfc5562 15 Set_vector3(omega,_zero3);
benson516 1:edc7ccfc5562 16
benson516 1:edc7ccfc5562 17 _init_flag = 0; // Uninitialized
benson516 1:edc7ccfc5562 18 Set_vector3(_x_est,_unit_nk);
benson516 1:edc7ccfc5562 19 Set_L1_diag(alpha);
benson516 1:edc7ccfc5562 20
benson516 0:8126c86bac2a 21
benson516 0:8126c86bac2a 22 }
benson516 1:edc7ccfc5562 23 // Public methods
benson516 1:edc7ccfc5562 24 void ATTITUDE::gVector_to_EulerAngle(float v_out[], float v_in[])
benson516 1:edc7ccfc5562 25 {
benson516 1:edc7ccfc5562 26 //
benson516 1:edc7ccfc5562 27 // This function should be customized according to the definition of coordinate system
benson516 1:edc7ccfc5562 28 //
benson516 1:edc7ccfc5562 29
benson516 1:edc7ccfc5562 30 // Here we follow the definition in bicycle paper
benson516 1:edc7ccfc5562 31 v_out[0] = 0.0; // phi, yaw
benson516 1:edc7ccfc5562 32 v_out[2] = atan2(-v_in[1],v_in[0]); // theta, roll
benson516 1:edc7ccfc5562 33 v_out[1] = atan2(cos(v_out[2])*v_in[2],v_in[0]); // psi, pitch
benson516 1:edc7ccfc5562 34 }
benson516 1:edc7ccfc5562 35 void ATTITUDE::Get_CrossProduct3(float v_c[], float v_a[], float v_b[]) // v_a X v_b
benson516 1:edc7ccfc5562 36 {
benson516 1:edc7ccfc5562 37 v_c[0] = (-v_a[2]*v_b[1]) + v_a[1]*v_b[2];
benson516 1:edc7ccfc5562 38 v_c[1] = v_a[2]*v_b[0] - v_a[0]*v_b[2];
benson516 1:edc7ccfc5562 39 v_c[2] = (-v_a[1]*v_b[0]) + v_a[0]*v_b[1];
benson516 1:edc7ccfc5562 40 }
benson516 1:edc7ccfc5562 41 float ATTITUDE::Get_Vector3Norm(float v_in[])
benson516 1:edc7ccfc5562 42 {
benson516 1:edc7ccfc5562 43 float temp = 0.0;
benson516 1:edc7ccfc5562 44
benson516 1:edc7ccfc5562 45 for(int i=0; i<3; i++)
benson516 1:edc7ccfc5562 46 temp += v_in[i]*v_in[i];
benson516 1:edc7ccfc5562 47 return sqrt(temp); // <- Should chech if this function is available (?)
benson516 1:edc7ccfc5562 48 }
benson516 1:edc7ccfc5562 49 void ATTITUDE::Init_Estimator(float y_in[]) // Let _x_est = y_in
benson516 1:edc7ccfc5562 50 {
benson516 1:edc7ccfc5562 51 Set_vector3(ys,y_in);
benson516 1:edc7ccfc5562 52 Set_vector3(_x_est,ys); // _x_est be set as y_in
benson516 1:edc7ccfc5562 53 _init_flag = 1;
benson516 1:edc7ccfc5562 54 }
benson516 1:edc7ccfc5562 55 void ATTITUDE::Run_Estimator(float y_in[], float omega_in[]) // Main alogorithm
benson516 1:edc7ccfc5562 56 {
benson516 1:edc7ccfc5562 57 static float x_est_plus[3];
benson516 1:edc7ccfc5562 58 static float cross_pruduct[3];
benson516 1:edc7ccfc5562 59
benson516 1:edc7ccfc5562 60 Set_vector3(ys,y_in);
benson516 1:edc7ccfc5562 61 Set_vector3(omega,omega_in);
benson516 1:edc7ccfc5562 62
benson516 1:edc7ccfc5562 63 if(_init_flag == 0)
benson516 1:edc7ccfc5562 64 Init_Estimator(ys);
benson516 1:edc7ccfc5562 65 else
benson516 1:edc7ccfc5562 66 {
benson516 1:edc7ccfc5562 67 Get_CrossProduct3(cross_pruduct,omega,ys);
benson516 1:edc7ccfc5562 68 for(int i=0; i<3; i++)
benson516 1:edc7ccfc5562 69 x_est_plus[i] = _x_est[i] + Ts*(_L1_diag[i]*(ys[i] - _x_est[i]) - cross_pruduct[i]);
benson516 1:edc7ccfc5562 70
benson516 1:edc7ccfc5562 71 Set_vector3(_x_est,x_est_plus);
benson516 1:edc7ccfc5562 72 }
benson516 1:edc7ccfc5562 73 //
benson516 1:edc7ccfc5562 74 gVector_to_EulerAngle(EulerAngle,_x_est);
benson516 1:edc7ccfc5562 75 }
benson516 1:edc7ccfc5562 76 void ATTITUDE::Get_Estimation(float v[])
benson516 1:edc7ccfc5562 77 {
benson516 1:edc7ccfc5562 78 Set_vector3(v,_x_est);
benson516 1:edc7ccfc5562 79 }
benson516 1:edc7ccfc5562 80 void ATTITUDE::Get_Attitude_Euler(float v[])
benson516 1:edc7ccfc5562 81 {
benson516 1:edc7ccfc5562 82 Set_vector3(v,EulerAngle);
benson516 1:edc7ccfc5562 83 }
benson516 1:edc7ccfc5562 84
benson516 1:edc7ccfc5562 85 // Private methods
benson516 1:edc7ccfc5562 86 void ATTITUDE::Set_vector3(float v_a[], float v_b[]) // Vectors in R^3. Let the values of v_b be set to v_a
benson516 1:edc7ccfc5562 87 {
benson516 1:edc7ccfc5562 88 for(int i=0; i<3; i++ )
benson516 1:edc7ccfc5562 89 v_a[i] = v_b[i];
benson516 1:edc7ccfc5562 90 }
benson516 1:edc7ccfc5562 91 void ATTITUDE::Set_L1_diag(float alpha) // set diagnal element of gain matrix
benson516 1:edc7ccfc5562 92 {
benson516 1:edc7ccfc5562 93 for(int i=0; i<3; i++)
benson516 1:edc7ccfc5562 94 _L1_diag[i] = alpha;
benson516 1:edc7ccfc5562 95 }
benson516 1:edc7ccfc5562 96