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

diff --git a/src/sensor_fusion/design/lib/estimate_orientation.m b/src/sensor_fusion/design/lib/estimate_orientation.m
index dfbbc18..6e96c89 100755 (executable)
--- 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 (executable)
--- 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 (executable)
--- 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 (executable)
--- 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