Updating sf_orientation implementation for quaternion

Updating the sf_orientation octave implementation to support
the orientation computed interms of quaternions instead of euler
angles.

Change-Id: I5dc5e12ba9e70dc41bc20ebfef97cbd69b2ab808

diff --git a/src/sensor_fusion/design/sf_orientation.m b/src/sensor_fusion/design/sf_orientation.m
index be1d107..a14916a 100755 (executable)
--- a/src/sensor_fusion/design/sf_orientation.m
+++ b/src/sensor_fusion/design/sf_orientation.m
@@ -26,11 +26,16 @@ 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;
 Max_Range_Magnetic = 1200; Min_Range_Magnetic = -1200; Res_Magnetic = 1;
 
+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;
@@ -57,6 +62,8 @@ OR_driv = zeros(3,BUFFER_SIZE);
 OR_aid = zeros(3,BUFFER_SIZE);
 OR_err = zeros(3,BUFFER_SIZE);
 
+euler = zeros(BUFFER_SIZE,3);
+
 % Sensor Data simulating orientation motions
 
 % get accel x,y,z axis data from stored file
@@ -83,7 +90,22 @@ 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
-[OR_driv, OR_aid, OR_err]  = estimate_orientation(Accel_data, Gyro_data, Mag_data);
+[Quat_driv, Quat_aid, Quat_err]  = estimate_orientation(Accel_data, Gyro_data, Mag_data);
+
+euler = quat2euler(Quat_aid);
+OR_aid(1,:) = euler(:,2)' * RAD2DEG;
+OR_aid(2,:) = euler(:,1)' * RAD2DEG;
+OR_aid(3,:) = euler(:,3)' * 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_err);
+OR_err(1,:) = euler(:,2)' * RAD2DEG;
+OR_err(2,:) = euler(:,1)' * RAD2DEG;
+OR_err(3,:) = euler(:,3)' * RAD2DEG;
 
 % Rotation Plot Results
 hfig=(figure);