Attitude estimation using IMU (3-DoF ver.)
Fork of ATTITUDE_ESTIMATION by
Diff: ATTITUDE_ESTIMATION.cpp
- 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); }