Diff: ATTITUDE_ESTIMATION.h

Revision:

6:c362ed165c39

Parent:

5:01e322f4158f

Child:

7:6fc812e342e6

--- a/ATTITUDE_ESTIMATION.h	Wed May 04 10:47:37 2016 +0000
+++ b/ATTITUDE_ESTIMATION.h	Sat Dec 24 09:51:20 2016 +0000
@@ -1,57 +1,121 @@
 #ifndef _ATTITUDE_ESTIMATION_H_
 #define _ATTITUDE_ESTIMATION_H_
+
 #include "mbed.h"
+#include <vector>
+
+using std::vector;
+
+
+class LPF_vector
+{public:
+    vector<float> output;
+
+    LPF_vector(size_t dimension, float samplingTime, float cutOff_freq_Hz_in); // cutOff_freq_Hz_in is in "Hz"
+    vector<float> filter(const vector<float> &v_in);
+    void reset(const vector<float> &v_in);
+
+private:
+    size_t n;
+    float Ts;
+    float cutOff_freq_Hz; // Hz
+    float alpha_Ts;
+    float One_alpha_Ts;
+
+    // Flag
+    bool Flag_Init;
+
+    //
+    vector<float> zeros; // Zero vector [0;0;0]
+};
+
 
 // Class
 class ATTITUDE{
 public:
-        
+
     // Variables
     float alpha; // Convergent rate, rad/sec.
+    float one_over_gamma; // 1/gamma, one_over_gamma == 0 means no estimation on gyro bias
     float Ts; // Sampling time, sec.
-    
-    float x_est[3]; // Estimated state
-    float EulerAngle[3];
-    float ys[3]; // Sensor output
-    float omega[3]; // Rotation speed in body-fixed frame
-    
+    bool enable_biasEst; // Enable the gyro-bias estimation capability
+
+    // The map from "real" coordinate to "here" coordinate
+    // eg. accMap_real2here = [3,-1,-2];
+    // means: real  ->  here
+    //     1   x         z   3
+    //     2   y        -x  -1
+    //     3   z        -y  -2
+    vector<int> accMap_real2here;
+    vector<int> gyroMap_real2here;
+
+    vector<float> x_est; // Estimated state
+    vector<float> gyroBias_est; // The estimated gyro bias in each channel
+    vector<float> omega; // Rotation speed in body-fixed frame
+    vector<float> ys; // Sensor output
+    vector<float> w_cross_ys; // omega X ys
+    vector<float> ys_cross_x_ys; // ys X (x_est - ys)
+
+    // Eular angles, in rad/s
+    float pitch;
+    float roll;
+    float yaw;
+
+
     // Constructor:
     // Initialize the estimator
-    ATTITUDE(float alpha_in, float Ts_in); // alpha in rad/sec., Ts in sec.
-    
+    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][]);
-    void gVector_to_EulerAngle(float v_out[],float v_in[]);
-    //float* EulerAngle_to_gVector(float Eu_in[]);   
-    
+    void gVector_to_EulerAngle(const vector<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[]);
-    
+    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);
+
     // Estimator
-    void Init(float y_in[]); // Let _x_est = y_in
-    void Run(float y_in[], float omega_in[]); // Main alogorithm
-    void Get_Estimation(float v[]);
-    void Get_Attitude_Euler(float v[]);
+    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);
+    void getEstimation_realCoordinate(vector<float> &V_out);
+    float pitch_deg(void);
+    float roll_deg(void);
+    float yaw_deg(void);
+
+    // Unit transformation
+    float pi; // pi = 3.1415926
+    float deg2rad; // = 3.1415926/180.0;
+    float rad2deg; // = 180.0/3.1415926;
 
 private:
+    // Dimension
+    size_t n;
+
     // Variables
-    float _unit_nz[3]; // (-z) direction [0;0;-1]
-    float _zero3[3]; // Zero vector [0;0;0]
+    vector<float> unit_nx; // (-x) direction [-1;0;0]
+    vector<float> unit_ny; // (-y) direction [0;-1;0]
+    vector<float> unit_nz; // (-z) direction [0;0;-1]
+    vector<float> zeros; // Zero vector [0;0;0]
     //
-    int8_t _init_flag; // Flag for displaying initialization status 
-    float _x_est[3]; // Estimated state
+    size_t init_flag; // Flag for displaying initialization status
     // float _omega_x[3][3]; // Skew symmetric matrix of omega
-    
-    
-    float _L1_diag[3]; // Diagonal vector of gain matrix L1
-    
-    
+
+    vector<float> L1_diag; // Diagonal vector of gain matrix L1
+
     // Methods
-    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
+    void Set_L1_diag(float alpha_in); // set diagnal element of gain matrix
 
+    LPF_vector lpfv_ys;
+    LPF_vector lpfv_w;
 };
-#endif // _ATTITUDE_ESTIMATION_H_
\ No newline at end of file
+
+#endif // _ATTITUDE_ESTIMATION_H_