Diff: ATTITUDE_ESTIMATION.h

Revision:

7:6fc812e342e6

Parent:

6:c362ed165c39

Child:

8:3882cb4be9d3

--- a/ATTITUDE_ESTIMATION.h	Sat Dec 24 09:51:20 2016 +0000
+++ b/ATTITUDE_ESTIMATION.h	Tue Dec 27 07:43:25 2016 +0000
@@ -67,25 +67,20 @@
     ATTITUDE(float alpha_in, float one_over_gamma_in, float Ts_in); // alpha in rad/sec., Ts in sec.
 
     // Methods
-    // Transformations
-    void InputMapping(vector<float> &v_hereDef, const vector<float> &v_realDef, const vector<int> &map_real2here);
-    void OutputMapping(vector<float> &v_realDef, const vector<float> &v_hereDef, const vector<int> &map_real2here);
-    //float* Vector_to_SkewSymmetry(float v_in[]);
-    //float* SkewSymmetry_to_Vector(float M_in[3][]);
+    // Get the estimation results in Eular angle
+    // vector <--> Eular angle
     void gVector_to_EulerAngle(const vector<float> &v_in);
     //float* EulerAngle_to_gVector(float Eu_in[]);
 
-    // vector operation
-    void Get_CrossProduct3(vector<float> &v_c, const vector<float> &v_a, const vector<float> &v_b); // v_a X v_b
-    vector<float> Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a + (or -) v_b
-    vector<float> Get_VectorScalarMultiply(const vector<float> &v_a, float scale); // scale*v_a
-    float Get_Vector3Norm(const vector<float> &v_in);
-    void Normolization(vector<float> &V_out, const vector<float> &V_in);
+    // Setting parameters
+    // Set L1, the diagonal matrix
+    void Set_L1_diag(float alpha_in); // set diagnal element of gain matrix
+
 
     // Estimator
     void Init(const vector<float> &y_in); // Let _x_est = y_in
     void iterateOnce(const vector<float> &y_in, const vector<float> &omega_in); // Main alogorithm
-    void updateGyroBiasEst(void);
+    // Get the results
     void getEstimation_realCoordinate(vector<float> &V_out);
     float pitch_deg(void);
     float roll_deg(void);
@@ -95,6 +90,7 @@
     float pi; // pi = 3.1415926
     float deg2rad; // = 3.1415926/180.0;
     float rad2deg; // = 180.0/3.1415926;
+    float gravity; // = 9.81 m/s^2
 
 private:
     // Dimension
@@ -111,9 +107,27 @@
 
     vector<float> L1_diag; // Diagonal vector of gain matrix L1
 
-    // Methods
-    void Set_L1_diag(float alpha_in); // set diagnal element of gain matrix
+    //
+    // Input/output coordinate transformations within the different definitions between the "real" one and the "here" one
+    void InputMapping(vector<float> &v_hereDef, const vector<float> &v_realDef, const vector<int> &map_real2here);
+    void OutputMapping(vector<float> &v_realDef, const vector<float> &v_hereDef, const vector<int> &map_real2here);
+
+
+    // The kernel of the estimation process
+    void EstimationKernel(vector<float> &_x_est_, const vector<float> &_ys_, const vector<float> &_omega_);
+    void updateGyroBiasEst(void);
 
+    // Utilities
+    // vector operation
+    //float* Vector_to_SkewSymmetry(float v_in[]);
+    //float* SkewSymmetry_to_Vector(float M_in[3][]);
+    void Get_CrossProduct3(vector<float> &v_c, const vector<float> &v_a, const vector<float> &v_b); // v_a X v_b
+    vector<float> Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a + (or -) v_b
+    vector<float> Get_VectorScalarMultiply(const vector<float> &v_a, float scale); // scale*v_a
+    float Get_Vector3Norm(const vector<float> &v_in);
+    void Normolization(vector<float> &V_out, const vector<float> &V_in);
+
+    // Low-pass filter, vector version
     LPF_vector lpfv_ys;
     LPF_vector lpfv_w;
 };