Attitude estimation using IMU (3-DoF ver.)

Fork of ATTITUDE_ESTIMATION by LDSC_Robotics_TAs

Revision:
13:16bc19155f54
Parent:
12:058a9edb664e
--- 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);
 }