From 75cd05b84e7351124af9e8d18916355b3708bf9f Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Tue, 6 Jan 2015 11:31:26 +0530 Subject: [PATCH] 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 --- src/sensor_fusion/design/sf_orientation.m | 24 +++++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/src/sensor_fusion/design/sf_orientation.m b/src/sensor_fusion/design/sf_orientation.m index be1d107..a14916a 100755 --- 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); -- 2.7.4