Adding octave code to compute gyro bias

- gyro bias is needed for calibrated gyroscope sensor event
- updated octave design code to simulate the gyroscope bias and plot it
- substituted euler error code(which showed only drift) with gyroscope
bias

Change-Id: I3e411db520d6103326c4e2950e05d6930e34dd9b

diff --git a/src/sensor_fusion/design/lib/estimate_orientation.m b/src/sensor_fusion/design/lib/estimate_orientation.m
index d7f023a..b2e3b73 100755 (executable)
--- a/src/sensor_fusion/design/lib/estimate_orientation.m
+++ b/src/sensor_fusion/design/lib/estimate_orientation.m
@@ -23,7 +23,7 @@
 % - Quaternion based approach
 % - Estimation and correction of orientation errors and bias errors for gyroscope using Kalman filter
 
-function [quat_driv, quat_aid, quat_error]  = estimate_orientation(Accel_data, Gyro_data, Mag_data)
+function [quat_driv, quat_aid, quat_error, gyro_bias]  = estimate_orientation(Accel_data, Gyro_data, Mag_data)
        PLOT_SCALED_SENSOR_COMPARISON_DATA = 1;
        PLOT_INDIVIDUAL_SENSOR_INPUT_DATA = 1;
 
@@ -42,6 +42,7 @@ function [quat_driv, quat_aid, quat_error]  = estimate_orientation(Accel_data, G
        GRAVITY = 9.80665;
        PI = 3.141593;
        NON_ZERO_VAL = 0.1;
+       PI_DEG = 180;
 
        MOVING_AVERAGE_WINDOW_LENGTH = 20;
        RAD2DEG = 57.2957795;
@@ -62,6 +63,8 @@ function [quat_driv, quat_aid, quat_error]  = estimate_orientation(Accel_data, G
        mag_z = zeros(1,BUFFER_SIZE);
        MTime = zeros(1,BUFFER_SIZE);
 
+       gyro_bias = zeros(3,BUFFER_SIZE);
+
        if MAG_DATA_DISABLED != 1
                Mx = Mag_data(1,:);
                My = Mag_data(2,:);
@@ -270,6 +273,7 @@ function [quat_driv, quat_aid, quat_error]  = estimate_orientation(Accel_data, G
                        Bx = x(4,i);
                        By = x(5,i);
                        Bz = x(6,i);
+                       gyro_bias(:,i) = [x1; x2; x3] * PI_DEG + [Bx; By; Bz];
                end
        end
 

diff --git a/src/sensor_fusion/design/sf_orientation.m b/src/sensor_fusion/design/sf_orientation.m
index 3a97580..1c181cb 100755 (executable)
--- a/src/sensor_fusion/design/sf_orientation.m
+++ b/src/sensor_fusion/design/sf_orientation.m
@@ -100,7 +100,11 @@ Mag_data(3,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")
 Mag_data(4,:) = ((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,4))')(1:BUFFER_SIZE);
 
 % estimate orientation
-[Quat_driv, Quat_aid, Quat_err]  = estimate_orientation(Accel_data, Gyro_data, Mag_data);
+[Quat_driv, Quat_aid, Quat_err, Gyro_bias]  = estimate_orientation(Accel_data, Gyro_data, Mag_data);
+
+Gyro_bias(1,:) = Gyro_bias(1,:) + Bias_Gx;
+Gyro_bias(2,:) = Gyro_bias(2,:) + Bias_Gy;
+Gyro_bias(3,:) = Gyro_bias(3,:) + Bias_Gz;
 
 for i = 1:BUFFER_SIZE
        euler_aid(i,:) = quat2euler(Quat_aid(i,:));
@@ -112,7 +116,7 @@ for i = 1:BUFFER_SIZE
        OR_driv(1,i) = ROLL_PHASE_CORRECTION * euler_driv(i,2)' * RAD2DEG;
        OR_driv(2,i) = PITCH_PHASE_CORRECTION * euler_driv(i,1)' * RAD2DEG;
        OR_driv(3,i) = YAW_PHASE_CORRECTION * euler_driv(i,3)' * RAD2DEG;
-       
+
        if (OR_driv(3,i) < 0)
                OR_driv(3,i) = OR_driv(3,i) + 360;
        end
@@ -120,11 +124,6 @@ for i = 1:BUFFER_SIZE
        if (OR_aid(3,i) < 0)
                OR_aid(3,i) = OR_aid(3,i) + 360;
        end
-
-       euler_err(i,:) = quat2euler(Quat_err(i,:));
-       OR_err(1,i) = euler_err(i,2)' * RAD2DEG;
-       OR_err(2,i) = euler_err(i,1)' * RAD2DEG;
-       OR_err(3,i) = euler_err(i,3)' * RAD2DEG;
 end
 
 % Rotation Plot Results
@@ -140,10 +139,10 @@ UA = OR_driv(2,:);
 p2 = plot(1:length(UA),UA(1,1:length(UA)),'b');
 hold on;
 grid on;
-UA = OR_err(2,:);
+UA = Gyro_bias(1,:);
 p3 = plot(1:length(UA),UA(1,1:length(UA)),'g');
 title(['Pitch']);
-legend([p1 p2 p3],'Aiding System', 'Driving System', 'Quaternion based error');
+legend([p1 p2 p3],'Aiding System', 'Driving System', 'Gyroscope Bias error');
 subplot(3,1,2)
 UA = OR_aid(1,:);
 p1 = plot(1:length(UA),UA(1,1:length(UA)),'r');
@@ -153,10 +152,10 @@ UA = OR_driv(1,:);
 p2 = plot(1:length(UA),UA(1,1:length(UA)),'b');
 hold on;
 grid on;
-UA = OR_err(1,:);
+UA = Gyro_bias(2,:);
 p3 = plot(1:length(UA),UA(1,1:length(UA)),'g');
 title(['Roll']);
-legend([p1 p2 p3],'Aiding System', 'Driving System', 'Quaternion based error');
+legend([p1 p2 p3],'Aiding System', 'Driving System', 'Gyroscope Bias error');
 subplot(3,1,3)
 UA = OR_aid(3,:);
 p1 = plot(1:length(UA),UA(1,1:length(UA)),'r');
@@ -166,7 +165,7 @@ UA = OR_driv(3,:);
 p2 = plot(1:length(UA),UA(1,1:length(UA)),'b');
 hold on;
 grid on;
-UA = OR_err(3,:);
+UA = Gyro_bias(3,:);
 p3 = plot(1:length(UA),UA(1,1:length(UA)),'g');
 title(['Yaw']);
-legend([p1 p2 p3],'Aiding System', 'Driving System', 'Quaternion based error');
+legend([p1 p2 p3],'Aiding System', 'Driving System', 'Gyroscope Bias error');