Diff: ATTITUDE_ESTIMATION.cpp

Revision:

1:edc7ccfc5562

Parent:

0:8126c86bac2a

Child:

2:46d355d7abaa

--- a/ATTITUDE_ESTIMATION.cpp	Thu Apr 28 09:38:58 2016 +0000
+++ b/ATTITUDE_ESTIMATION.cpp	Thu Apr 28 23:45:24 2016 +0000
@@ -1,6 +1,96 @@
 #include "ATTITUDE_ESTIMATION.h"
 
-ATTITUDE::ATTITUDE()
+ATTITUDE::ATTITUDE(float alpha_in, float Ts_in):alpha(alpha_in), Ts(Ts_in)
 {
+    //_unit_nk
+    //_zero3
+    float v1[3]={0.0,0.0,-1.0};
+    float v2[3]={0.0,0.0,0.0};   
+    Set_vector3(_unit_nk,v1);
+    Set_vector3(_zero3,v2);
+    
+    
+    Set_vector3(EulerAngle,_zero3);    
+    Set_vector3(ys,_zero3);    
+    Set_vector3(omega,_zero3);    
+    
+    _init_flag = 0; // Uninitialized
+    Set_vector3(_x_est,_unit_nk);    
+    Set_L1_diag(alpha);
+    
     
 }
+// Public methods
+void ATTITUDE::gVector_to_EulerAngle(float v_out[], float v_in[])
+{
+    //
+    // This function should be customized according to the definition of coordinate system
+    //
+    
+    // Here we follow the definition in bicycle paper
+    v_out[0] = 0.0; // phi, yaw
+    v_out[2] = atan2(-v_in[1],v_in[0]); // theta, roll
+    v_out[1] = atan2(cos(v_out[2])*v_in[2],v_in[0]); // psi, pitch
+}
+void ATTITUDE::Get_CrossProduct3(float v_c[], float v_a[], float v_b[]) // v_a X v_b
+{
+    v_c[0] = (-v_a[2]*v_b[1]) + v_a[1]*v_b[2];
+    v_c[1] = v_a[2]*v_b[0] - v_a[0]*v_b[2];
+    v_c[2] = (-v_a[1]*v_b[0]) + v_a[0]*v_b[1]; 
+}
+float ATTITUDE::Get_Vector3Norm(float v_in[])
+{
+    float temp = 0.0;
+    
+    for(int i=0; i<3; i++)
+        temp += v_in[i]*v_in[i];
+    return sqrt(temp); // <- Should chech if this function is available (?)
+}
+void ATTITUDE::Init_Estimator(float y_in[]) // Let _x_est = y_in
+{
+    Set_vector3(ys,y_in);
+    Set_vector3(_x_est,ys); // _x_est be set as y_in
+    _init_flag = 1;    
+}
+void ATTITUDE::Run_Estimator(float y_in[], float omega_in[]) // Main alogorithm
+{
+    static float x_est_plus[3];
+    static float cross_pruduct[3];
+    
+    Set_vector3(ys,y_in);
+    Set_vector3(omega,omega_in);
+    
+    if(_init_flag == 0)
+        Init_Estimator(ys);
+    else
+    {
+        Get_CrossProduct3(cross_pruduct,omega,ys);
+        for(int i=0; i<3; i++)
+            x_est_plus[i] = _x_est[i] + Ts*(_L1_diag[i]*(ys[i] - _x_est[i]) - cross_pruduct[i]);
+             
+        Set_vector3(_x_est,x_est_plus);
+    }
+    //
+    gVector_to_EulerAngle(EulerAngle,_x_est);
+}
+void ATTITUDE::Get_Estimation(float v[])
+{
+    Set_vector3(v,_x_est);
+}
+void ATTITUDE::Get_Attitude_Euler(float v[])
+{
+    Set_vector3(v,EulerAngle);
+}
+
+// Private methods
+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   
+{
+    for(int i=0; i<3; i++ )
+        v_a[i] = v_b[i];
+}
+void ATTITUDE::Set_L1_diag(float alpha) // set diagnal element of gain matrix
+{
+    for(int i=0; i<3; i++)
+        _L1_diag[i] = alpha;
+}
+