Diff: AttitudeEstimator/AttitudeEstimator.cpp

Revision:

1:24effec9e9aa

Parent:

0:b1f2c9e88e32

Child:

5:b9947e3d20cf

--- a/AttitudeEstimator/AttitudeEstimator.cpp	Fri Aug 31 18:41:31 2018 +0000
+++ b/AttitudeEstimator/AttitudeEstimator.cpp	Wed Sep 12 00:18:15 2018 +0000
@@ -15,40 +15,6 @@
     calibrate_gyro();
 }
 
-// Estimate euler angles (rad) and angular velocity (rad/s)
-void AttitudeEstimator::estimate()
-{
-    // Read IMU sensor data
-    imu.read();
-    // Estimate Euler angles (rad) and angular velocities (rad/s) from accelerometer and gyroscope data
-    estimate_accel();
-    estimate_gyro();
-    // 
-    phi = phi_accel*rho + phi_gyro*(1.0f-rho);
-    theta = theta_accel*rho + theta_gyro*(1.0f-rho);
-}
-
-// Estimate Euler angles (rad) from accelerometer data
-void AttitudeEstimator::estimate_accel()
-{
-    // Estimate Euler angles (rad)
-    phi_accel = atan2(-imu.ay,-imu.az);
-    theta_accel = atan2(imu.ax,-imu.az);
-}
-
-// Estimate Euler angles (rad) and angular velocities (rad/s) from gyroscope data
-void AttitudeEstimator::estimate_gyro()
-{
-    // Estimate angular velocities (rad/s)
-    p = imu.gx - p_bias;
-    q = imu.gy - q_bias;
-    r = imu.gz - r_bias;
-    // Estimate Euler angles (rad)
-    phi_gyro = phi + p*dt;
-    theta_gyro = theta + q*dt;
-    psi = psi + r*dt;
-}
-
 // Calibrates gyroscope by calculating angular velocity bias (rad/s)
 void AttitudeEstimator::calibrate_gyro()
 {
@@ -65,4 +31,65 @@
         r_bias += imu.gz/200.0f;
         wait(0.005);
     }
+}
+
+// Estimate euler angles (rad) and angular velocity (rad/s)
+void AttitudeEstimator::estimate()
+{
+    // Read IMU sensor data
+    imu.read();
+    // Estimate Euler angles (rad) and angular velocities (rad/s) from accelerometer and gyroscope data
+    estimate_accel();
+    estimate_gyro();
+    // Ponderate Euler angles (rad) estimation from accelerometer and gyroscope
+    phi = phi_accel*rho + phi_gyro*(1.0f-rho);
+    theta = theta_accel*rho + theta_gyro*(1.0f-rho);
+    psi = psi_gyro;
+}
+
+// Estimate Euler angles (rad) from accelerometer data
+void AttitudeEstimator::estimate_accel()
+{
+    // Estimate Euler angles (rad)
+    phi_accel = atan2(-imu.ay,-imu.az);
+    theta_accel = atan2(imu.ax,-((imu.az>0)-(imu.az<0))*sqrt(pow(imu.ay,2)+pow(imu.az,2)));
+}
+
+// Estimate Euler angles (rad) and angular velocities (rad/s) from gyroscope data
+void AttitudeEstimator::estimate_gyro()
+{
+    // Estimate angular velocities (rad/s)
+    p = imu.gx - p_bias;
+    q = imu.gy - q_bias;
+    r = imu.gz - r_bias;
+    // Estimate Euler angles (rad)
+    phi_gyro = phi + (p+q*sin(phi)*tan(theta)+r*cos(phi)*tan(theta))*dt;
+    theta_gyro = theta + (q*cos(phi)-r*sin(phi))*dt;
+    psi_gyro = psi + (q*sin(phi)/cos(theta)+r*cos(phi)/cos(theta))*dt;
+    // Adjust Euler angles (rad) to be in the interval of +/- pi
+    if(phi_gyro > pi)
+    {
+        phi_gyro = phi_gyro - 2*pi;
+    }
+    else if(phi_gyro < -pi)
+    {
+        phi_gyro = phi_gyro + 2*pi;
+    }
+    
+    if(theta_gyro > pi)
+    {
+        theta_gyro = theta_gyro - 2*pi;
+    }
+    else if(theta_gyro < -pi)
+    {
+        theta_gyro = theta_gyro + 2*pi;
+    }
+    if(psi_gyro > pi)
+    {
+        psi_gyro = psi_gyro - 2*pi;
+    }
+    else if(psi_gyro < -pi)
+    {
+        psi_gyro = psi_gyro + 2*pi;
+    }
 }
\ No newline at end of file