From e60cd9cf11fddd18ad58a2ed235012f09cd0e36d Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Wed, 7 Jan 2015 10:25:38 +0530 Subject: [PATCH] Modifying sf_orientation for new estimate_orientation - Adapting sf_orientation octave file to the restructured estmate_orientation ocatave implementation Change-Id: I4febde2c476182ebd6df9b186efc3ad7cbdc0dd2 --- src/sensor_fusion/design/sf_orientation.m | 34 +++++++++++++++++-------------- 1 file changed, 19 insertions(+), 15 deletions(-) diff --git a/src/sensor_fusion/design/sf_orientation.m b/src/sensor_fusion/design/sf_orientation.m index a14916a..140487a 100755 --- a/src/sensor_fusion/design/sf_orientation.m +++ b/src/sensor_fusion/design/sf_orientation.m @@ -62,7 +62,9 @@ OR_driv = zeros(3,BUFFER_SIZE); OR_aid = zeros(3,BUFFER_SIZE); OR_err = zeros(3,BUFFER_SIZE); -euler = zeros(BUFFER_SIZE,3); +euler_driv = zeros(BUFFER_SIZE,3); +euler_aid = zeros(BUFFER_SIZE,3); +euler_err = zeros(BUFFER_SIZE,3); % Sensor Data simulating orientation motions @@ -92,20 +94,22 @@ Mag_data(4,:) = ((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")( % estimate orientation [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; +for i = 1:BUFFER_SIZE + euler_aid(i,:) = quat2euler(Quat_aid(i,:)); + OR_aid(1,i) = euler_aid(i,2)' * RAD2DEG; + OR_aid(2,i) = euler_aid(i,1)' * RAD2DEG; + OR_aid(3,i) = euler_aid(i,3)' * RAD2DEG; + + euler_driv(i,:) = quat2euler(Quat_driv(i,:)); + 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; + + 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 hfig=(figure); -- 2.7.4