Diff: ATTITUDE_ESTIMATION.cpp

Revision:

8:3882cb4be9d3

Parent:

7:6fc812e342e6

Child:

9:84fad91d3587

--- a/ATTITUDE_ESTIMATION.cpp	Tue Dec 27 07:43:25 2016 +0000
+++ b/ATTITUDE_ESTIMATION.cpp	Tue Dec 27 11:28:49 2016 +0000
@@ -45,8 +45,9 @@
                         alpha(alpha_in),
                         one_over_gamma(one_over_gamma_in),
                         Ts(Ts_in),
-                        lpfv_ys(3, Ts_in, 10.0), // Input filter
-                        lpfv_w(3, Ts_in, 200.0) // Input filter
+                        lpfv_y_acce(3, Ts_in, 10.0), // Input filter for accelerometers
+                        lpfv_y_mag(3, Ts_in, 200.0), // Input filter for magenetometers
+                        lpfv_w(3, Ts_in, 200.0) // Input filter for gyroscope
 {
     // Dimension
     n = 3;
@@ -55,6 +56,7 @@
 
     // Default: close the gyro-bias estimation
     enable_biasEst = false;
+    enable_magEst = false;
 
     // Unit transformation
     pi = 3.1415926;
@@ -72,8 +74,10 @@
     int accmap_temp[] = {-3,1,2}; // Reverse: The direction of accelerometer is defined based on the direction of the acceleration of the sensor, not the g-direction
     // int accmap_temp[] = {1, 2, 3};
     //
+    int magMap_temp[] = {3,-1, 2}; // real z-axis is in the reverse direction
     int gyroMap_temp[] = {3,-1,-2};
     accMap_real2here.assign(accmap_temp, accmap_temp+n);
+    magMap_real2here.assign(magMap_temp, magMap_temp+n);
     gyroMap_real2here.assign(gyroMap_temp, gyroMap_temp+n);
 
     // zeros
@@ -87,11 +91,15 @@
     unit_nz[2] = -1; // negative z
 
     // States
-    x_est = unit_nx; // x is pointing downward
+    xg_est = unit_nx; // g is pointing downward
+    xm_est = Get_VectorScalarMultiply(unit_nz, -1.0); // m is pointing forward
     gyroBias_est = zeros;
     omega = zeros;
-    ys = zeros;
-    w_cross_ys = zeros; // omega X ys
+    //
+    y_acce = zeros; // Accelerometer outputs
+    y_mag = zeros; // Magnetometer outputs
+    //
+    // w_cross_ys = zeros; // omega X ys
     ys_cross_x_ys = zeros; // ys X (x_est - ys)
 
     // Eular angles, in rad/s
@@ -105,8 +113,7 @@
 
 }
 // Public methods
-void ATTITUDE::gVector_to_EulerAngle(const vector<float> &v_in)
-{
+void ATTITUDE::Vectors_to_EulerAngle(const vector<float> &vg_in, const vector<float> &vm_in){
     //
     // This function should be customized according to the definition of coordinate system
     //
@@ -114,23 +121,30 @@
     /*
     // Here we follow the definition in bicycle paper
     yaw = 0.0; // phi, yaw
-    roll = atan2(-v_in[1],v_in[0]); // theta, roll
-    pitch = atan2(cos(roll)*v_in[2],v_in[0]); // psi, pitch
+    roll = atan2(-vg_in[1],vg_in[0]); // theta, roll
+    pitch = atan2(cos(roll)*vg_in[2],vg_in[0]); // psi, pitch
     */
 
     // Eular angle: 1-3-2, zs is pointing forwasd
-    yaw = 0.0; // phi, yaw
-    pitch = atan2(-v_in[2],-v_in[0]); // psi, pitch
+    // yaw = 0.0; // phi, yaw
+    pitch = atan2(-vg_in[2],-vg_in[0]); // psi, pitch
     if (abs(pitch) <  0.7854){ // pi/4
-        roll = atan2(cos(pitch)*v_in[1],-v_in[0]); // theta, roll
+        roll = atan2(cos(pitch)*vg_in[1],-vg_in[0]); // theta, roll
     }else{
         if (pitch >= 0.0)
-            roll = atan2(sin(pitch)*v_in[1],-v_in[2]); // theta, roll
+            roll = atan2(sin(pitch)*vg_in[1],-vg_in[2]); // theta, roll
         else
-            roll = atan2(-sin(pitch)*v_in[1],v_in[2]); // theta, roll
+            roll = atan2(-sin(pitch)*vg_in[1],vg_in[2]); // theta, roll
     }
 
-
+    // Calculate the yaw angle
+    if (enable_magEst){
+        float num = vm_in[1]*cos(pitch);
+        float den = (vm_in[2]*cos(roll) - vm_in[1]*sin(pitch)*sin(roll));
+        yaw = atan2(num, den);
+    }else{
+        yaw = 0.0; // phi, yaw
+    }
 
 }
 // Setting parameters
@@ -141,42 +155,40 @@
     L1_diag.assign(n,alpha_in);
 }
 //
-void ATTITUDE::Init(const vector<float> &y_in) // Let _x_est = y_in
+void ATTITUDE::Init(void) // Let x_est = ys
 {
-    // ys = y_in;
-    // Normolization(x_est,y_in); // x_est be set as normalized y_in
-    x_est = y_in;
+    // y_acce = y_in;
+    // Normolization(xg_est,y_in); // xg_est be set as normalized y_in
+    xg_est = y_acce;
+    xm_est = y_mag;
     ++init_flag;
 }
-void ATTITUDE::iterateOnce(const vector<float> &y_in, const vector<float> &omega_in) // Main alogorithm
+void ATTITUDE::iterateOnce(const vector<float> &y_acce_in, const vector<float> &omega_in) // Main alogorithm
 {
-    InputMapping(ys, y_in, accMap_real2here);
+    enable_magEst = false; // no magenetometers input
+    // Input mapping
+    InputMapping(y_acce, y_acce_in, accMap_real2here);
+    // InputMapping(y_mag, y_mag_in, magMap_real2here);
     InputMapping(omega, omega_in, gyroMap_real2here);
 
     // Input filter
-    ys = lpfv_ys.filter(ys);
+    y_acce = lpfv_y_acce.filter(y_acce);
+    // y_mag = lpfv_y_mag.filter(y_mag);
     // omega = lpfv_w.filter(omega);
 
     // gyro-bias estimation
     if (enable_biasEst){
-        omega = Get_VectorPlus(omega,gyroBias_est, true); // omega - gyroBias_est
+        omega = Get_VectorPlus(omega, gyroBias_est, true); // minus, omega - gyroBias_est
     }
 
     //
     if(init_flag < 3){
-        Init(ys);
+        Init();
     }
     else{
-        //
-        /*
-        Get_CrossProduct3(w_cross_ys,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]) - w_cross_ys[i]);
-            x_est[i] += Ts*( L1_diag[i]*(ys[i] - x_est[i]) - w_cross_ys[i]);
-        }
-        */
-        // Estimation process
-        EstimationKernel(x_est, ys, omega);
+        // Estimation kernel process
+        EstimationKernel(xg_est, y_acce, omega);
+        // EstimationKernel(xm_est, y_mag, omega);
 
         // gyro-bias estimation
         if (enable_biasEst){
@@ -184,11 +196,46 @@
         }
     }
     //
-    gVector_to_EulerAngle(x_est);
+    Vectors_to_EulerAngle(xg_est,xm_est);
+}
+void ATTITUDE::iterateOnce(const vector<float> &y_acce_in, const vector<float> &y_mag_in, const vector<float> &omega_in) // Main alogorithm
+{
+    enable_magEst = true; // with magenetometers input
+    // Input mapping
+    InputMapping(y_acce, y_acce_in, accMap_real2here);
+    InputMapping(y_mag, y_mag_in, magMap_real2here);
+    InputMapping(omega, omega_in, gyroMap_real2here);
+
+    // Input filter
+    y_acce = lpfv_y_acce.filter(y_acce);
+    // y_mag = lpfv_y_mag.filter(y_mag);
+    // omega = lpfv_w.filter(omega);
+
+    // gyro-bias estimation
+    if (enable_biasEst){
+        omega = Get_VectorPlus(omega, gyroBias_est, true); // minus, omega - gyroBias_est
+    }
+
+    //
+    if(init_flag < 3){
+        Init();
+    }
+    else{
+        // Estimation kernel process
+        EstimationKernel(xg_est, y_acce, omega);
+        EstimationKernel(xm_est, y_mag, omega);
+
+        // gyro-bias estimation
+        if (enable_biasEst){
+            updateGyroBiasEst();
+        }
+    }
+    //
+    Vectors_to_EulerAngle(xg_est,xm_est);
 }
 // transform the x_est into "real" coordinate
 void ATTITUDE::getEstimation_realCoordinate(vector<float> &V_out){
-    OutputMapping(V_out,x_est,accMap_real2here);
+    OutputMapping(V_out,xg_est,accMap_real2here);
 }
 // Get Eular angles
 float ATTITUDE::pitch_deg(void){
@@ -255,7 +302,7 @@
 void ATTITUDE::EstimationKernel(vector<float> &_x_est_, const vector<float> &_ys_, const vector<float> &_omega_){
     static vector<float> _w_cross_ys_;
     Get_CrossProduct3(_w_cross_ys_, _omega_, _ys_);
-    for(int i=0; i<3; i++){
+    for(size_t i = 0; i < n; ++i){
         // x_est_plus[i] = x_est[i] + Ts*( L1_diag[i]*(ys[i] - x_est[i]) - w_cross_ys[i]);
         _x_est_[i] += Ts*( L1_diag[i]*(_ys_[i] - _x_est_[i]) - _w_cross_ys_[i]);
     }
@@ -266,7 +313,7 @@
         return;
     }
     //
-    Get_CrossProduct3(ys_cross_x_ys, ys, Get_VectorPlus(x_est,ys,true));
+    Get_CrossProduct3(ys_cross_x_ys, y_acce, Get_VectorPlus(xg_est,y_acce,true));
     //
     gyroBias_est = Get_VectorPlus(gyroBias_est, Get_VectorScalarMultiply(ys_cross_x_ys, (one_over_gamma)), true);
 }