Diff: ATTITUDE_ESTIMATION.h

Revision:

8:3882cb4be9d3

Parent:

7:6fc812e342e6

Child:

9:84fad91d3587

--- a/ATTITUDE_ESTIMATION.h	Tue Dec 27 07:43:25 2016 +0000
+++ b/ATTITUDE_ESTIMATION.h	Tue Dec 27 11:28:49 2016 +0000
@@ -39,6 +39,7 @@
     float one_over_gamma; // 1/gamma, one_over_gamma == 0 means no estimation on gyro bias
     float Ts; // Sampling time, sec.
     bool enable_biasEst; // Enable the gyro-bias estimation capability
+    bool enable_magEst; // Enable the estimation of magenetic field
 
     // The map from "real" coordinate to "here" coordinate
     // eg. accMap_real2here = [3,-1,-2];
@@ -47,13 +48,20 @@
     //     2   y        -x  -1
     //     3   z        -y  -2
     vector<int> accMap_real2here;
+    vector<int> magMap_real2here;
     vector<int> gyroMap_real2here;
 
-    vector<float> x_est; // Estimated state
+    vector<float> xg_est; // Estimated g-vector
+    vector<float> xm_est; // Estimated m-vector (magnetic field)
+    // 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> y_acce; // Accelerometer outputs
+    vector<float> y_mag; // Magnetometer outputs
+
+    // No use, may be deleted
+    // vector<float> w_cross_ys; // omega X ys
     vector<float> ys_cross_x_ys; // ys X (x_est - ys)
 
     // Eular angles, in rad/s
@@ -69,7 +77,7 @@
     // Methods
     // Get the estimation results in Eular angle
     // vector <--> Eular angle
-    void gVector_to_EulerAngle(const vector<float> &v_in);
+    void Vectors_to_EulerAngle(const vector<float> &vg_in, const vector<float> &vm_in);
     //float* EulerAngle_to_gVector(float Eu_in[]);
 
     // Setting parameters
@@ -78,8 +86,10 @@
 
 
     // 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 Init(void); // Let x_est = ys
+    // Two version of estimator, with/without magenetic field estimation
+    void iterateOnce(const vector<float> &y_acce_in, const vector<float> &omega_in); // Main alogorithm
+    void iterateOnce(const vector<float> &y_acce_in, const vector<float> &y_mag_in, const vector<float> &omega_in); // Main alogorithm
     // Get the results
     void getEstimation_realCoordinate(vector<float> &V_out);
     float pitch_deg(void);
@@ -128,7 +138,8 @@
     void Normolization(vector<float> &V_out, const vector<float> &V_in);
 
     // Low-pass filter, vector version
-    LPF_vector lpfv_ys;
+    LPF_vector lpfv_y_acce;
+    LPF_vector lpfv_y_mag;
     LPF_vector lpfv_w;
 };