From d12da5029a27ab8c2bda699fa2129cb035c5f862 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Mon, 15 Sep 2014 11:21:20 +0530 Subject: [PATCH] Updating octave design files - Updating octave design files based on testing of RD-PQ target data. - Adding customization data for when code is being ported to multiple devices. Change-Id: I7c6b352102c3686a6b90f10103b668d2852c262a --- src/sensor_fusion/design/lib/estimate_orientation.m | 12 ++++++++---- src/sensor_fusion/design/sf_gravity.m | 16 ++++++++++++---- src/sensor_fusion/design/sf_linear_acceleration.m | 14 +++++++++++--- src/sensor_fusion/design/sf_orientation.m | 14 +++++++++++--- 4 files changed, 42 insertions(+), 14 deletions(-) diff --git a/src/sensor_fusion/design/lib/estimate_orientation.m b/src/sensor_fusion/design/lib/estimate_orientation.m index dfbbc18..6e96c89 100755 --- a/src/sensor_fusion/design/lib/estimate_orientation.m +++ b/src/sensor_fusion/design/lib/estimate_orientation.m @@ -25,6 +25,10 @@ function [OR_driv, OR_aid, OR_err] = estimate_orientation(Accel_data, Gyro_data LOW_PASS_FILTERING_ON = 0; 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; @@ -124,7 +128,7 @@ function [OR_driv, OR_aid, OR_err] = estimate_orientation(Accel_data, Gyro_data M_T(1) = 100000; acc_e = [0.0;0.0;1.0]; % gravity vector in earth frame - mag_e = [0.0;1.0;0.0]; % magnetic field vector in earth frame + mag_e = [0.0;MAGNETIC_ALIGNMENT_FACTOR;0.0]; % magnetic field vector in earth frame H = [eye(3) zeros(3,3); zeros(3,6)]; x = zeros(6,BUFFER_SIZE); @@ -303,9 +307,9 @@ function [OR_driv, OR_aid, OR_err] = estimate_orientation(Accel_data, Gyro_data OR_aid(3,:) = yaw * RAD2DEG; euler = quat2euler(quat_driv); - OR_driv(1,:) = euler(:,2)' * RAD2DEG; - OR_driv(2,:) = euler(:,1)' * RAD2DEG; - OR_driv(3,:) = -euler(:,3)' * RAD2DEG; + 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; diff --git a/src/sensor_fusion/design/sf_gravity.m b/src/sensor_fusion/design/sf_gravity.m index 19f249c..07e47d6 100755 --- a/src/sensor_fusion/design/sf_gravity.m +++ b/src/sensor_fusion/design/sf_gravity.m @@ -39,6 +39,14 @@ Bias_Gx = -5.3539; Bias_Gy = 0.24325; Bias_Gz = 2.3391; +Bias_Mx = 0; +Bias_My = 0; +Bias_Mz = 0; + +Sign_Mx = 1; +Sign_My = 1; +Sign_Mz = 1; + BUFFER_SIZE = 100; Accel_data = zeros(4,BUFFER_SIZE); @@ -69,9 +77,9 @@ Gyro_data(2,:) = Gyro_data(2,:)/scale_Gyro; Gyro_data(3,:) = Gyro_data(3,:)/scale_Gyro; % get magnetometer x,y,z axis data from stored file -Mag_data(1,:) = (((dlmread("data/100ms/gravity/single_roll_throw/magnetic.txt")(:,1))'))(1:BUFFER_SIZE); -Mag_data(2,:) = (((dlmread("data/100ms/gravity/single_roll_throw/magnetic.txt")(:,2))'))(1:BUFFER_SIZE); -Mag_data(3,:) = (((dlmread("data/100ms/gravity/single_roll_throw/magnetic.txt")(:,3))'))(1:BUFFER_SIZE); +Mag_data(1,:) = (((dlmread("data/100ms/gravity/single_roll_throw/magnetic.txt")(:,1))') + Bias_Mx)(1:BUFFER_SIZE) * Sign_Mx; +Mag_data(2,:) = (((dlmread("data/100ms/gravity/single_roll_throw/magnetic.txt")(:,2))') + Bias_My)(1:BUFFER_SIZE) * Sign_My; +Mag_data(3,:) = (((dlmread("data/100ms/gravity/single_roll_throw/magnetic.txt")(:,3))') + Bias_Mz)(1:BUFFER_SIZE) * Sign_Mz; Mag_data(4,:) = ((dlmread("data/100ms/gravity/single_roll_throw/magnetic.txt")(:,4))')(1:BUFFER_SIZE); % estimate orientation @@ -92,7 +100,7 @@ hold on; grid on; UA = Accel_data(3,:); p3 = plot(1:length(UA),UA(1,1:length(UA)),'r'); -title(['Accelerometer Raw Data']); +title(['Raw Accelerometer Data']); legend([p1 p2 p3],'x-axis', 'y-axis', 'z-axis'); % Gravity Plot Results diff --git a/src/sensor_fusion/design/sf_linear_acceleration.m b/src/sensor_fusion/design/sf_linear_acceleration.m index edb78e0..21555b4 100755 --- a/src/sensor_fusion/design/sf_linear_acceleration.m +++ b/src/sensor_fusion/design/sf_linear_acceleration.m @@ -39,6 +39,14 @@ Bias_Gx = -5.3539; Bias_Gy = 0.24325; Bias_Gz = 2.3391; +Bias_Mx = 0; +Bias_My = 0; +Bias_Mz = 0; + +Sign_Mx = 1; +Sign_My = 1; +Sign_Mz = 1; + BUFFER_SIZE = 125; Accel_data = zeros(4,BUFFER_SIZE); @@ -69,9 +77,9 @@ Gyro_data(2,:) = Gyro_data(2,:)/scale_Gyro; Gyro_data(3,:) = Gyro_data(3,:)/scale_Gyro; % get magnetometer x,y,z axis data from stored file -Mag_data(1,:) = (((dlmread("data/100ms/linear_acceleration/move_x_y_z/magnetic.txt")(:,1))'))(1:BUFFER_SIZE); -Mag_data(2,:) = (((dlmread("data/100ms/linear_acceleration/move_x_y_z/magnetic.txt")(:,2))'))(1:BUFFER_SIZE); -Mag_data(3,:) = (((dlmread("data/100ms/linear_acceleration/move_x_y_z/magnetic.txt")(:,3))'))(1:BUFFER_SIZE); +Mag_data(1,:) = (((dlmread("data/100ms/linear_acceleration/move_x_y_z/magnetic.txt")(:,1))') + Bias_Mx)(1:BUFFER_SIZE) * Sign_Mx; +Mag_data(2,:) = (((dlmread("data/100ms/linear_acceleration/move_x_y_z/magnetic.txt")(:,2))') + Bias_My)(1:BUFFER_SIZE) * Sign_My; +Mag_data(3,:) = (((dlmread("data/100ms/linear_acceleration/move_x_y_z/magnetic.txt")(:,3))') + Bias_Mz)(1:BUFFER_SIZE) * Sign_Mz; Mag_data(4,:) = ((dlmread("data/100ms/linear_acceleration/move_x_y_z/magnetic.txt")(:,4))')(1:BUFFER_SIZE); % estimate orientation diff --git a/src/sensor_fusion/design/sf_orientation.m b/src/sensor_fusion/design/sf_orientation.m index d1c927c..be1d107 100755 --- a/src/sensor_fusion/design/sf_orientation.m +++ b/src/sensor_fusion/design/sf_orientation.m @@ -39,6 +39,14 @@ Bias_Gx = -5.3539; Bias_Gy = 0.24325; Bias_Gz = 2.3391; +Bias_Mx = 0; +Bias_My = 0; +Bias_Mz = 0; + +Sign_Mx = 1; +Sign_My = 1; +Sign_Mz = 1; + BUFFER_SIZE = 1095; Accel_data = zeros(4,BUFFER_SIZE); @@ -69,9 +77,9 @@ Gyro_data(2,:) = Gyro_data(2,:)/scale_Gyro; Gyro_data(3,:) = Gyro_data(3,:)/scale_Gyro; % get magnetometer x,y,z axis data from stored file -Mag_data(1,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,1))'))(1:BUFFER_SIZE); -Mag_data(2,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,2))'))(1:BUFFER_SIZE); -Mag_data(3,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,3))'))(1:BUFFER_SIZE); +Mag_data(1,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,1))') + Bias_Mx)(1:BUFFER_SIZE) * Sign_Mx; +Mag_data(2,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,2))') + Bias_My)(1:BUFFER_SIZE) * Sign_My; +Mag_data(3,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,3))') + Bias_Mz)(1:BUFFER_SIZE) * Sign_Mz; Mag_data(4,:) = ((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,4))')(1:BUFFER_SIZE); % estimate orientation -- 2.7.4