From: Ramasamy <ram.kannan@samsung.com>
Date: Thu, 2 Jul 2015 00:50:55 +0000 (+0900)
Subject: Improving the sensor fusion algorithm for orientation using accel & gyro
X-Git-Tag: submit/tizen/20151218.070016~57^2
X-Git-Url: http://review.tizen.org/git/?a=commitdiff_plain;h=876d33fad9b40ce337cf8e001743a1dd6a4d10e1;p=platform%2Fcore%2Fsystem%2Fsensord.git

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 511b646..de28616 100755
--- 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 892b86e..b8bb8b4 100755
--- 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 1c181cb..0d6f3e0 100755
--- 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;