Chun Feng Huang / ATTITUDE_ESTIMATION_WIPV

Attitude estimation using IMU (3-DoF ver.)

Fork of ATTITUDE_ESTIMATION by LDSC_Robotics_TAs

Files at this revision

API Documentation at this revision

Revision 13:16bc19155f54, committed 2017-01-20

Comitter:
benson516
Date:
Fri Jan 20 15:46:48 2017 +0000
Parent:
12:058a9edb664e
Commit message:
Remove the filtering action on y_acce

Changed in this revision

ATTITUDE_ESTIMATION.cpp Show annotated file Show diff for this revision Revisions of this file
ATTITUDE_ESTIMATION.h Show annotated file Show diff for this revision Revisions of this file 
--- a/ATTITUDE_ESTIMATION.cpp	Thu Jan 05 11:14:20 2017 +0000
+++ b/ATTITUDE_ESTIMATION.cpp	Fri Jan 20 15:46:48 2017 +0000
@@ -101,6 +101,9 @@
     //
     y_acce = zeros; // Accelerometer outputs
     y_mag = zeros; // Magnetometer outputs
+
+    // Linear acceleration estimation
+    acce_sensorFrame = zeros;
     //
     // w_cross_ys = zeros; // omega X ys
     ys_cross_x_ys = zeros; // ys X (x_est - ys)
@@ -196,7 +199,7 @@
     InputMapping(omega, omega_in, gyroMap_real2here);
 
     // Input filter
-    y_acce = lpfv_y_acce.filter(y_acce);
+    // y_acce = lpfv_y_acce.filter(y_acce);
     // y_mag = lpfv_y_mag.filter(y_mag);
     // omega = lpfv_w.filter(omega);
 
@@ -219,6 +222,10 @@
             updateGyroBiasEst();
         }
     }
+
+    // Calculate the linear acceleration
+    acce_sensorFrame = Get_VectorPlus(y_acce, xg_est, true); // minus
+    
     //
     Vectors_to_EulerAngle(xg_est,xm_est);
 }
@@ -231,7 +238,7 @@
     InputMapping(omega, omega_in, gyroMap_real2here);
 
     // Input filter
-    y_acce = lpfv_y_acce.filter(y_acce);
+    // y_acce = lpfv_y_acce.filter(y_acce);
     // y_mag = lpfv_y_mag.filter(y_mag);
     // omega = lpfv_w.filter(omega);
 
@@ -254,6 +261,9 @@
             updateGyroBiasEst();
         }
     }
+
+    // Calculate the linear acceleration
+    acce_sensorFrame = Get_VectorPlus(y_acce, xg_est, true); // minus
     //
     Vectors_to_EulerAngle(xg_est,xm_est);
 }

--- a/ATTITUDE_ESTIMATION.h	Thu Jan 05 11:14:20 2017 +0000
+++ b/ATTITUDE_ESTIMATION.h	Fri Jan 20 15:46:48 2017 +0000
@@ -61,6 +61,9 @@
     vector<float> y_acce; // Accelerometer outputs
     vector<float> y_mag; // Magnetometer outputs
 
+    // Linear acceleration estimation
+    vector<float> acce_sensorFrame; // The residual of acceleration, acce_sensorFrame = y_acce - xg_est
+
     // No use, may be deleted
     // vector<float> w_cross_ys; // omega X ys
     vector<float> ys_cross_x_ys; // ys X (x_est - ys)