From: Ramasamy <ram.kannan@samsung.com>
Date: Tue, 6 Jan 2015 05:37:12 +0000 (+0530)
Subject: Updating estimate_orientation call to output quaternions
X-Git-Tag: submit/tizen/20150113.012540~11^2~4
X-Git-Url: http://review.tizen.org/git/?a=commitdiff_plain;h=4c5b0d23176039f8f122b03747c0cb5aa59af3ea;p=platform%2Fcore%2Fsystem%2Fsensord.git

Updating estimate_orientation call to output quaternions

- Updating the estimate_orientation ocatave implementation to output
orientation values in terms of quaternions instead of euler angles.
- cleanup

Change-Id: Ida5a39a3270eafb26807a1bf218f3d48c32d789b
---

diff --git a/src/sensor_fusion/design/lib/estimate_orientation.m b/src/sensor_fusion/design/lib/estimate_orientation.m
index d20ac6c5..187ef384 100755
--- a/src/sensor_fusion/design/lib/estimate_orientation.m
+++ b/src/sensor_fusion/design/lib/estimate_orientation.m
@@ -18,17 +18,13 @@
 %
 % - Orientation Estimation using Gyroscope as the driving system and Accelerometer+Geo-Magnetic Sensors as Aiding System.
 % - Quaternion based approach
-% - Estimation and correction of Euler errors and bias errors for gyroscope using Kalman filter
+% - Estimation and correction of orientation errors and bias errors for gyroscope using Kalman filter
 
-function [OR_driv, OR_aid, OR_err]  = estimate_orientation(Accel_data, Gyro_data, Mag_data)
-
-	LOW_PASS_FILTERING_ON = 0;
+function [quat_driv, quat_aid, quat_error]  = estimate_orientation(Accel_data, Gyro_data, Mag_data)
 	PLOT_SCALED_SENSOR_COMPARISON_DATA = 0;
 	PLOT_INDIVIDUAL_SENSOR_INPUT_DATA = 0;
+
 	MAGNETIC_ALIGNMENT_FACTOR = -1;
-	PITCH_PHASE_CORRECTION = -1;
-	ROLL_PHASE_CORRECTION = -1;
-	YAW_PHASE_CORRECTION = -1;
 
 	GRAVITY = 9.80665;
 	PI = 3.141593;
@@ -95,12 +91,6 @@ function [OR_driv, OR_aid, OR_err]  = estimate_orientation(Accel_data, Gyro_data
 	quat_driv = zeros(BUFFER_SIZE,4);
 	quat_aid = zeros(BUFFER_SIZE,4);
 	quat_error = zeros(BUFFER_SIZE,4);
-	euler = zeros(BUFFER_SIZE,3);
-	Ro = zeros(3, 3, BUFFER_SIZE);
-
-	OR_driv = zeros(3,BUFFER_SIZE);
-	OR_aid = zeros(3,BUFFER_SIZE);
-	OR_err = zeros(3,BUFFER_SIZE);
 
 	% system covariance matrix
 	Q = zeros(6,6);
@@ -252,21 +242,6 @@ function [OR_driv, OR_aid, OR_err]  = estimate_orientation(Accel_data, Gyro_data
 		By = x(5,i);
 		Bz = x(6,i);
 	end
-
-	OR_aid(1,:) = roll * RAD2DEG;
-	OR_aid(2,:) = pitch * RAD2DEG;
-	OR_aid(3,:) = yaw * RAD2DEG;
-
-	euler = quat2euler(quat_driv);
-	OR_driv(1,:) = ROLL_PHASE_CORRECTION * euler(:,2)' * RAD2DEG;
-	OR_driv(2,:) = PITCH_PHASE_CORRECTION * euler(:,1)' * RAD2DEG;
-	OR_driv(3,:) = YAW_PHASE_CORRECTION * euler(:,3)' * RAD2DEG;
-
-	euler = quat2euler(quat_error);
-	OR_err(1,:) = euler(:,2)' * RAD2DEG;
-	OR_err(2,:) = euler(:,1)' * RAD2DEG;
-	OR_err(3,:) = euler(:,3)' * RAD2DEG;
-
 	if PLOT_SCALED_SENSOR_COMPARISON_DATA == 1
 		% Accelerometer/Gyroscope/Magnetometer scaled Plot results
 		hfig=(figure);