Improving the sensor fusion algorithm for orientation using accel & gyro

- Improving algorithm by correcting gyroscope orientation using limited
accelerometer orientation.
- Change made and tested only on octave design code.

Change-Id: Ic6abd19ab9f64cb06149622b11533eb1754279aa

diff --git a/src/sensor_fusion/design/lib/estimate_orientation.m b/src/sensor_fusion/design/lib/estimate_orientation.m
index 511b646d8d77e055da291832330ac284735ab28c..de28616a4faec5532d9c61a1e9bd6e2570ee0542 100755 (executable)
--- a/src/sensor_fusion/design/lib/estimate_orientation.m
+++ b/src/sensor_fusion/design/lib/estimate_orientation.m
@@ -42,6 +42,7 @@ function [quat_driv, quat_aid, quat_error, gyro_bias]  = estimate_orientation(Ac
        GRAVITY = 9.80665;
        PI = 3.141593;
        NON_ZERO_VAL = 0.1;
+       ROUNDOFF_VAL = 0.0025;
        PI_DEG = 180;
 
        MOVING_AVERAGE_WINDOW_LENGTH = 20;
@@ -220,7 +221,6 @@ function [quat_driv, quat_aid, quat_error, gyro_bias]  = estimate_orientation(Ac
                                var_yaw(i) = var(yaw((i-MOVING_AVERAGE_WINDOW_LENGTH):i));
                        end
 
-
                        Qwn = [var_Gx(i) 0 0;0 var_Gy(i) 0;0 0 var_Gz(i);];
                        Qwb = (2 * (ZigmaW^2) / TauW) * eye(3);
 
@@ -254,10 +254,13 @@ function [quat_driv, quat_aid, quat_error, gyro_bias]  = estimate_orientation(Ac
                                q = q / norm(q);
                        else
                                euler_aid = quat2euler(quat_aid(i,:));
-                               euler_driv = quat2euler(quat_driv(i,:));
-
-                               euler_gaming_rv = [euler_aid(2) euler_aid(1) euler_driv(3)];
-                               quat_gaming_rv(i,:) = euler2quat(euler_gaming_rv);
+                               if (euler_aid(1)^2 < (ROUNDOFF_VAL * PI))
+                                       if (euler_aid(2)^2 < (ROUNDOFF_VAL * PI))
+                                               if (gyr_z(i)^2 < (NON_ZERO_VAL))
+                                                       q = quat_aid(i,:);
+                                               end
+                                       end
+                               end
                        end
 
                        if i > 1
@@ -280,8 +283,8 @@ function [quat_driv, quat_aid, quat_error, gyro_bias]  = estimate_orientation(Ac
                end
        end
 
-       if MAG_DATA_DISABLED == 1
-               quat_driv = quat_gaming_rv;
+       if GYRO_DATA_DISABLED == 1
+               quat_driv = quat_aid;
        end
 
        if PLOT_SCALED_SENSOR_COMPARISON_DATA == 1

diff --git a/src/sensor_fusion/design/sf_gaming_rv.m b/src/sensor_fusion/design/sf_gaming_rv.m
index 892b86e168903388568535f5c12cd76d4fce4eb5..b8bb8b40aa4bd6f362ec8f97e57e279d5076f2d8 100755 (executable)
--- a/src/sensor_fusion/design/sf_gaming_rv.m
+++ b/src/sensor_fusion/design/sf_gaming_rv.m
@@ -26,10 +26,15 @@ close all
 clc
 
 GRAVITY = 9.80665;
+RAD2DEG = 57.2957795;
 
 Max_Range_Accel = 39.203407; Min_Range_Accel = -39.204006; Res_Accel = 0.000598;
 Max_Range_Gyro = 1146.862549; Min_Range_Gyro = -1146.880005; Res_Gyro = 0.017500;
 
+PITCH_PHASE_CORRECTION = -1;
+ROLL_PHASE_CORRECTION = -1;
+YAW_PHASE_CORRECTION = -1;
+
 Bias_Ax = 0.098586;
 Bias_Ay = 0.18385;
 Bias_Az = 10.084 - GRAVITY;
@@ -70,6 +75,9 @@ Game_RV = estimate_gaming_rv(Accel_data, Gyro_data);
 
 for i = 1:BUFFER_SIZE
        Orientation_RV(:,i) = quat2euler(Game_RV(i,:));
+       Orientation_RV(1,i) = ROLL_PHASE_CORRECTION * Orientation_RV(1,i) * RAD2DEG;
+       Orientation_RV(2,i) = PITCH_PHASE_CORRECTION * Orientation_RV(2,i) * RAD2DEG;
+       Orientation_RV(3,i) = YAW_PHASE_CORRECTION * Orientation_RV(3,i) * RAD2DEG;
 end
 
 hfig=(figure);

diff --git a/src/sensor_fusion/design/sf_orientation.m b/src/sensor_fusion/design/sf_orientation.m
index 1c181cbe9707eb6b22bfc6ccf73791c11c36391a..0d6f3e0e0bdb3dcaebe411019cc7b362b3c42e3b 100755 (executable)
--- a/src/sensor_fusion/design/sf_orientation.m
+++ b/src/sensor_fusion/design/sf_orientation.m
@@ -88,7 +88,7 @@ Gyro_data(2,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/gyro.txt")(:,
 Gyro_data(3,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/gyro.txt")(:,3))') - Bias_Gz)(1:BUFFER_SIZE);
 Gyro_data(4,:) = ((dlmread("data/100ms/orientation/roll_pitch_yaw/gyro.txt")(:,4))')(1:BUFFER_SIZE);
 
-scale_Gyro = 1150;
+scale_Gyro = 575;
 Gyro_data(1,:) = Gyro_data(1,:)/scale_Gyro;
 Gyro_data(2,:) = Gyro_data(2,:)/scale_Gyro;
 Gyro_data(3,:) = Gyro_data(3,:)/scale_Gyro;