Diff: ATTITUDE_ESTIMATION.h

Revision:

1:edc7ccfc5562

Parent:

0:8126c86bac2a

Child:

2:46d355d7abaa

--- a/ATTITUDE_ESTIMATION.h	Thu Apr 28 09:38:58 2016 +0000
+++ b/ATTITUDE_ESTIMATION.h	Thu Apr 28 23:45:24 2016 +0000
@@ -2,51 +2,54 @@
 #define _ATTITUDE_ESTIMATION_H_
 #include "mbed.h"
 
-typedef struct{
-    float e[3];
-    } VECTOR3;
-typedef struct{
-    float e[3][3];
-    } MATRIX33;
-
 // Class
 class ATTITUDE{
 public:
         
     // Variables
-    VECTOR3 x_est;
-    VECTOR3 EulerAngle;
+    float alpha; // Convergent rate, rad/sec.
+    float Ts; // Sampling time, sec.
     
+    float EulerAngle[3];
+    float ys[3]; // Sensor output
+    float omega[3]; // Rotation speed in body-fixed frame
     
     // Constructor:
-    // Initialize the estimator, 
-    ATTITUDE();
+    // Initialize the estimator
+    ATTITUDE(float alpha_in, float Ts_in); // alpha in rad/sec., Ts in sec.
     
     // Methods
     // Transformations
-    MATRIX33 Vector_to_SkewSymmetry(VECTOR3);
-    VECTOR3 SkewSymmetry_to_Vector(MATRIX33);
-    VECTOR3 gVector_to_EulerAngle(VECTOR3);
-    VECTOR3 EulerAngle_to_gVector(VECTOR3);   
+    //float* Vector_to_SkewSymmetry(float v_in[]);
+    //float* SkewSymmetry_to_Vector(float M_in[3][]);
+    void gVector_to_EulerAngle(float v_out[],float v_in[]);
+    //float* EulerAngle_to_gVector(float Eu_in[]);   
+    
+    // vector operation
+    void Get_CrossProduct3(float v_c[],float v_a[], float v_b[]); // v_c =  v_a X v_b
+    float Get_Vector3Norm(float v_in[]);
+    
     // Estimator
-    void Init_Estimator(VECTOR3);
-    void Reset_Estimator(VECTOR3);
-    void Run_Estimator(VECTOR3);
+    void Init_Estimator(float y_in[]); // Let _x_est = y_in
+    void Run_Estimator(float y_in[], float omega_in[]); // Main alogorithm
+    void Get_Estimation(float v[]);
+    void Get_Attitude_Euler(float v[]);
 
 private:
+    float _unit_nk[3]; // (-k) direction [0;0;-1]
+    float _zero3[3]; // Zero vector [0;0;0]
+    //
     int8_t _init_flag; // Flag for displaying initialization status 
-    VECTOR3 _x_est; // Estimated state
-    VECTOR3 _y; // Sensor output
-    VECTOR3 _omega; // Rotation speed in body-fixed frame
-    // MATRIX33 _Omega_cross; // Skew-symetric matrix of rotaion speed
+    float _x_est[3]; // Estimated state
+    // float _omega_x[3][3]; // Skew symmetric matrix of omega
     
-    MATRIX33 _L1; // Gain matrix
-    float _Ts; // Sampling time
+    
+    float _L1_diag[3]; // Diagonal vector of gain matrix L1
+    
     
     // Method
-    void Set_x_est(VECTOR3);
-    void Set_measurements(float[],float[]); // array -> VECTOR33; y, omega
-    void Set_L1(MATRIX33); // set gain matrix
+    void Set_vector3(float v_a[], float v_b[]); // Vectors in R^3. Let the values of v_b be set to v_a   
+    void Set_L1_diag(float alpha); // set diagnal element of gain matrix
 
 };
 #endif // _ATTITUDE_ESTIMATION_H_
\ No newline at end of file