Sensor Fusion Orientation Estimation update

Updated the following changes from the initial version submitted
  - Kalman Updates based on noise covariance from aiding and driving system
  - Added better time update transformation matrix
  - Added support for Bias estimation and updation
  - Cleanup

signed-off-by: Ramasamy <ram.kannan@samsung.com>
Change-Id: I69b930d6c15649857d9a5fd9aa23e48fd0e486d4

diff --git a/SensorFusion/Quat2Euler.m b/SensorFusion/Quat2Euler.m
index b4495ee..20d244b 100755 (executable)
--- a/SensorFusion/Quat2Euler.m
+++ b/SensorFusion/Quat2Euler.m
@@ -29,4 +29,4 @@ function euler = Quat2Euler(q)
     psi = atan2(R(2,1,:), R(1,1,:) );\r
 \r
     euler = [phi(1,:)' theta(1,:)' psi(1,:)'];\r
-end\r
+end
\ No newline at end of file

diff --git a/SensorFusion/Quat2RotMat.m b/SensorFusion/Quat2RotMat.m
index b253862..1e91d1d 100755 (executable)
--- a/SensorFusion/Quat2RotMat.m
+++ b/SensorFusion/Quat2RotMat.m
@@ -27,3 +27,4 @@ function R = Quat2RotMat(q)
     R(3,2,:) = 2.*(q(:,3).*q(:,4)-q(:,1).*q(:,2));\r
     R(3,3,:) = 2.*q(:,1).^2-1+2.*q(:,4).^2;\r
 end\r
+\r

diff --git a/SensorFusion/QuatProd.m b/SensorFusion/QuatProd.m
index dbd5682..114f244 100755 (executable)
--- a/SensorFusion/QuatProd.m
+++ b/SensorFusion/QuatProd.m
@@ -22,3 +22,4 @@ function ab = QuatProd(a, b)
     ab(:,3) = a(:,1).*b(:,3)-a(:,2).*b(:,4)+a(:,3).*b(:,1)+a(:,4).*b(:,2);\r
     ab(:,4) = a(:,1).*b(:,4)+a(:,2).*b(:,3)-a(:,3).*b(:,2)+a(:,4).*b(:,1);\r
 end\r
+\r

diff --git a/SensorFusion/RotMat2Quat.m b/SensorFusion/RotMat2Quat.m
index 87693bc..f10caa5 100755 (executable)
--- a/SensorFusion/RotMat2Quat.m
+++ b/SensorFusion/RotMat2Quat.m
@@ -41,4 +41,4 @@ function q = RotMat2Quat(R)
         q(i,:) = V(:,4)';
         q(i,:) = [q(i,4) q(i,1) q(i,2) q(i,3)];
     end
-end
+end
\ No newline at end of file

diff --git a/SensorFusion/SF_Orien.m b/SensorFusion/SF_Orien.m
index 5906b3b..66a708a 100755 (executable)
--- a/SensorFusion/SF_Orien.m
+++ b/SensorFusion/SF_Orien.m
@@ -14,7 +14,7 @@
 % See the License for the specific language governing permissions and\r
 % limitations under the License.\r
 \r
-% Sensor Fusion Implementation - Orientation Estimation\r
+% Sensor Fusion Implementation - Orientation Estimation \r
 %\r
 % - Orientation Estimation using Gyroscope as the driving system and Accelerometer+Geo-Magnetic Sensors as Aiding System.\r
 % - Quaternion based approach\r
@@ -23,7 +23,7 @@
 \r
 clear\r
 \r
-LOW_PASS_FILTERING_ON = 0;\r
+LOW_PASS_FILTERING_ON = 1;\r
 \r
 Max_Range_Accel = 39.203407; Min_Range_Accel = -39.204006; Res_Accel = 0.000598;\r
 Max_Range_Gyro = 1146.862549; Min_Range_Gyro = -1146.880005; Res_Gyro = 0.017500;\r
@@ -41,11 +41,22 @@ Bias_Gx = -5.3539;
 Bias_Gy = 0.24325;\r
 Bias_Gz = 2.3391;\r
 \r
-BUFFER_SIZE = 1065;\r
+BUFFER_SIZE = 1095;\r
 MOVING_AVERAGE_WINDOW_LENGTH = 20;\r
-RAD = 57.2957795;\r
+RAD2DEG = 57.2957795;\r
+DEG2RAD = 0.0174532925;\r
 US2S =  1.0 / 1000000.0;\r
 \r
+% Gyro Types\r
+% Systron-donner "Horizon"\r
+ZigmaW = 0.05 * DEG2RAD; %deg/s \r
+TauW = 1000; %secs \r
+% Crossbow DMU-6X\r
+%ZigmaW = 0.05 * DEG2RAD; %deg/s \r
+%TauW = 300; %secs \r
+%FOGs (KVH Autogyro and Crossbow DMU-FOG)\r
+%ZigmaW = 0; %deg/s  \r
+\r
 % get accel x,y,z axis data from stored file\r
 Ax = (((dlmread("sensor_data/100ms/roll_pitch_yaw/accel.txt")(:,1))') - Bias_Ax)(1:BUFFER_SIZE);\r
 Ay = (((dlmread("sensor_data/100ms/roll_pitch_yaw/accel.txt")(:,2))') - Bias_Ay)(1:BUFFER_SIZE);\r
@@ -69,9 +80,12 @@ My = (((dlmread("sensor_data/100ms/roll_pitch_yaw/magnetic.txt")(:,2))'))(1:BUFF
 Mz = (((dlmread("sensor_data/100ms/roll_pitch_yaw/magnetic.txt")(:,3))'))(1:BUFFER_SIZE);\r
 MTime = ((dlmread("sensor_data/100ms/roll_pitch_yaw/magnetic.txt")(:,4))');\r
 \r
+% Gyroscope Bias Variables\r
+Bx = 0; By = 0; Bz = 0;\r
+\r
 % 1st order smoothening filter\r
-b = [0.9999   0 ];\r
-a = [1.0000000  0.1 ];\r
+b = [0.98   0 ];\r
+a = [1.0000000  0.02 ];\r
 \r
 % initialize filter output variables to zero\r
 filt_Ax = zeros(1,BUFFER_SIZE);\r
@@ -94,72 +108,46 @@ mag_x = zeros(1,BUFFER_SIZE);
 mag_y = zeros(1,BUFFER_SIZE);\r
 mag_z = zeros(1,BUFFER_SIZE);\r
 \r
-UA = zeros(1,BUFFER_SIZE);\r
-Corrected_Az = zeros(1,BUFFER_SIZE);\r
-\r
 % User Acceleration mean and Variance\r
-mean_UA = zeros(1,BUFFER_SIZE);\r
 A_T = zeros(1,BUFFER_SIZE);\r
 G_T = zeros(1,BUFFER_SIZE);\r
 M_T = zeros(1,BUFFER_SIZE);\r
-var_UA = zeros(1,BUFFER_SIZE);\r
-var_Ax = zeros(1,BUFFER_SIZE);\r
-var_Ay = zeros(1,BUFFER_SIZE);\r
-var_Az = zeros(1,BUFFER_SIZE);\r
+var_roll = zeros(1,BUFFER_SIZE);\r
+var_pitch = zeros(1,BUFFER_SIZE);\r
+var_yaw = zeros(1,BUFFER_SIZE);\r
 var_Gx = zeros(1,BUFFER_SIZE);\r
 var_Gy = zeros(1,BUFFER_SIZE);\r
 var_Gz = zeros(1,BUFFER_SIZE);\r
-var_Mx = zeros(1,BUFFER_SIZE);\r
-var_My = zeros(1,BUFFER_SIZE);\r
-var_Mz = zeros(1,BUFFER_SIZE);\r
-\r
-mean_Roll = zeros(1,BUFFER_SIZE);\r
-mean_Pitch = zeros(1,BUFFER_SIZE);\r
-mean_Yaw = zeros(1,BUFFER_SIZE);\r
-var_Roll = zeros(1,BUFFER_SIZE);\r
-var_Pitch = zeros(1,BUFFER_SIZE);\r
-var_Yaw = zeros(1,BUFFER_SIZE);\r
+\r
+roll = zeros(1,BUFFER_SIZE);\r
+pitch = zeros(1,BUFFER_SIZE);\r
+yaw = zeros(1,BUFFER_SIZE);\r
 quat_driv = zeros(BUFFER_SIZE,4);\r
 quat_aid = zeros(BUFFER_SIZE,4);\r
 quat_error = zeros(BUFFER_SIZE,4);\r
 euler = zeros(BUFFER_SIZE,3);\r
 Ro = zeros(3, 3, BUFFER_SIZE);\r
 \r
-% Gyroscope Bias Variables\r
-Bx = 0; By = 0; Bz = 0;\r
-\r
 % system covariance matrix\r
-Q=[ 1.1 0  0   0 0 0;\r
-       0  1.1 0   0 0 0;\r
-       0  0   1.1 0 0 0;\r
-       0  0   0   0 0 0;\r
-       0  0   0   0 0 0;\r
-       0  0   0   0 0 0;];\r
+Q = zeros(6,6);\r
 \r
 % measurement covariance matrix\r
-R=[ 1.01 0    0    0    0    0;\r
-       0    1.01 0    0    0    0;\r
-       0    0    1.01 0    0    0;\r
-       0    0    0    1.01 0    0;\r
-       0    0    0    0    1.01 0;\r
-       0    0    0    0    0    1.01;];\r
-\r
-%R=[ 1.01 0    0;\r
-%      0    1.01 0;\r
-%      0    0    1.01;];\r
+R = zeros(6,6);\r
        \r
 A_T(1) = 100000;\r
 G_T(1) = 100000;\r
 M_T(1) = 100000;\r
        \r
 acc_e = [0.0;0.0;1.0]; % gravity vector in earth frame\r
-mag_e = [0.0;1.0;0.0]; % magnetic field vector in eart frame   \r
+mag_e = [0.0;1.0;0.0]; % magnetic field vector in earth frame  \r
        \r
 H = [eye(3) zeros(3,3); zeros(3,6)];\r
 x = zeros(6,BUFFER_SIZE);\r
 e = zeros(1,6);\r
 P = 1 * eye(6);% state covariance matrix\r
 \r
+quat_driv(1,:) = [1 0 0 0];\r
+\r
 % first order filtering\r
 for i = 1:BUFFER_SIZE\r
        if LOW_PASS_FILTERING_ON == 1\r
@@ -214,30 +202,43 @@ for i = 1:BUFFER_SIZE
        \r
        UA(i) = sqrt(acc_x(i)^2 + acc_y(i)^2 + acc_z(i)^2) - GRAVITY;\r
        \r
+       gyr_x(i) = gyr_x(i) - Bx;\r
+       gyr_y(i) = gyr_y(i) - By;\r
+       gyr_z(i) = gyr_z(i) - Bz;\r
+       \r
+       % Aiding System (Accelerometer + Geomagnetic) quaternion generation\r
+       % gravity vector in body frame\r
+       acc_b =[acc_x(i);acc_y(i);acc_z(i)]; \r
+       % magnetic field vector in body frame\r
+       mag_b =[mag_x(i);mag_y(i);mag_z(i)];\r
+\r
+       % compute measurement quaternion with TRIAD algorithm\r
+       acc_b_x_mag_b = cross(acc_b,mag_b);\r
+       acc_e_x_mag_e = cross(acc_e,mag_e);\r
+       M_b = [acc_b acc_b_x_mag_b cross(acc_b_x_mag_b,acc_b)];\r
+       M_e = [acc_e acc_e_x_mag_e cross(acc_e_x_mag_e,acc_e)];\r
+       Rot_m = M_e * M_b';     \r
+       quat_aid(i,:) = RotMat2Quat(Rot_m);\r
+       \r
+       euler = Quat2Euler(quat_aid(i,:));\r
+       roll(i) = euler(1);\r
+       pitch(i) = euler(2);\r
+       yaw(i) = euler(3);\r
+       \r
        if i <= MOVING_AVERAGE_WINDOW_LENGTH\r
-               mean_UA(i) = UA(i);\r
-               var_UA(i) = NON_ZERO_VAL;\r
-               var_Ax(i) = NON_ZERO_VAL;\r
-               var_Ay(i) = NON_ZERO_VAL;\r
-               var_Az(i) = NON_ZERO_VAL;\r
                var_Gx(i) = NON_ZERO_VAL;\r
                var_Gy(i) = NON_ZERO_VAL;\r
                var_Gz(i) = NON_ZERO_VAL;\r
-               var_Mx(i) = NON_ZERO_VAL;\r
-               var_My(i) = NON_ZERO_VAL;\r
-               var_Mz(i) = NON_ZERO_VAL;\r
+               var_roll(i) = NON_ZERO_VAL;\r
+               var_pitch(i) = NON_ZERO_VAL;\r
+               var_yaw(i) = NON_ZERO_VAL;\r
        else\r
-               mean_UA(i) = mean(UA((i-MOVING_AVERAGE_WINDOW_LENGTH):i));\r
-               var_UA(i) = var(UA((i-MOVING_AVERAGE_WINDOW_LENGTH):i));\r
-               var_Ax(i) = var(acc_x((i-MOVING_AVERAGE_WINDOW_LENGTH):i));\r
-               var_Ay(i) = var(acc_y((i-MOVING_AVERAGE_WINDOW_LENGTH):i));\r
-               var_Az(i) = var(acc_z((i-MOVING_AVERAGE_WINDOW_LENGTH):i));\r
                var_Gx(i) = var(gyr_x((i-MOVING_AVERAGE_WINDOW_LENGTH):i));\r
                var_Gy(i) = var(gyr_y((i-MOVING_AVERAGE_WINDOW_LENGTH):i));\r
                var_Gz(i) = var(gyr_z((i-MOVING_AVERAGE_WINDOW_LENGTH):i));\r
-               var_Mx(i) = var(mag_x((i-MOVING_AVERAGE_WINDOW_LENGTH):i));\r
-               var_My(i) = var(mag_y((i-MOVING_AVERAGE_WINDOW_LENGTH):i));\r
-               var_Mz(i) = var(mag_z((i-MOVING_AVERAGE_WINDOW_LENGTH):i));\r
+               var_roll(i) = var(roll((i-MOVING_AVERAGE_WINDOW_LENGTH):i));\r
+               var_pitch(i) = var(pitch((i-MOVING_AVERAGE_WINDOW_LENGTH):i));\r
+               var_yaw(i) = var(yaw((i-MOVING_AVERAGE_WINDOW_LENGTH):i));\r
        end\r
        if i > 1\r
                A_T(i) = ATime(i) - ATime(i-1);\r
@@ -247,23 +248,14 @@ for i = 1:BUFFER_SIZE
        \r
        dt = G_T(i) * US2S;\r
        \r
-       gyr_x(i) = gyr_x(i) + Bx;\r
-       gyr_y(i) = gyr_y(i) + By;\r
-       gyr_z(i) = gyr_z(i) + Bz;\r
+       Qwn = [var_Gx(i) 0 0;0 var_Gy(i) 0;0 0 var_Gz(i);];\r
+       Qwb = (2 * (ZigmaW^2) / TauW) * eye(3);\r
        \r
-       % Aiding System (Accelerometer + Geomagnetic) quaternion generation\r
-       % gravity vector in body frame\r
-       acc_b =[acc_x(i);acc_y(i);acc_z(i)]; \r
-       % magnetic field vector in body frame\r
-       mag_b =[mag_x(i);mag_y(i);mag_z(i)]; \r
-\r
-       % compute measurement quaternion with TRIAD algorithm\r
-       acc_b_x_mag_b = cross(acc_b,mag_b);\r
-       acc_e_x_mag_e = cross(acc_e,mag_e);\r
-       M_b = [acc_b acc_b_x_mag_b cross(acc_b_x_mag_b,acc_b)];\r
-       M_e = [acc_e acc_e_x_mag_e cross(acc_e_x_mag_e,acc_e)];\r
-       Rot_m = M_e * M_b';     \r
-       quat_aid(i,:) = RotMat2Quat(Rot_m);\r
+       % Process Noise Covariance\r
+       Q = [Qwn zeros(3,3);zeros(3,3) Qwb];\r
+       \r
+       % Measurement Noise Covariance\r
+       R = [[var_roll(i) 0 0;0 var_pitch(i) 0;0 0 var_yaw(i)] zeros(3,3); zeros(3,3) zeros(3,3)];\r
        \r
        % initialization for q\r
        if i == 1\r
@@ -272,10 +264,7 @@ for i = 1:BUFFER_SIZE
        \r
        q_t = [q(2) q(3) q(4)]';\r
        Rtan = (q(1)^2 - q_t'*q_t)*eye(3) + 2*q_t*q_t' - 2*q(1)*[0 -q(4) q(3);q(4) 0 -q(2);-q(3) q(2) 0];\r
-       %F = [[0 gyr_z(i) -gyr_y(i);-gyr_z(i) 0 gyr_x(i);gyr_y(i) -gyr_x(i) 0] Rtan;  eye(3) ((-1/dt) * eye(3))];\r
-       F = [[0 gyr_z(i) -gyr_y(i);-gyr_z(i) 0 gyr_x(i);gyr_y(i) -gyr_x(i) 0] Rtan;  zeros(3,6)];\r
-       %F = [[0 gyr_z(i) -gyr_y(i);-gyr_z(i) 0 gyr_x(i);gyr_y(i) -gyr_x(i) 0] Rtan;  zeros(3,3) ((1/dt) * eye(3))];\r
-       %F = eye(6); \r
+       F = [[0 gyr_z(i) -gyr_y(i);-gyr_z(i) 0 gyr_x(i);gyr_y(i) -gyr_x(i) 0] Rtan; zeros(3,3) (-(1/TauW) * eye(3))];\r
 \r
        % Time Update\r
        if i > 1\r
@@ -306,9 +295,9 @@ for i = 1:BUFFER_SIZE
        q = QuatProd(quat_driv(i,:), [1 x1 x2 x3]) * PI;\r
        q = q / norm(q);\r
        \r
-       By = x(4,i);\r
-       Bx = x(5,i);\r
-       Bz = x(6,i);\r
+       Bx = Bx + x(4,i);\r
+       By = By + x(5,i);\r
+       Bz = Bz + x(6,i);\r
 \r
        if i > 1\r
                e = [x1 x2 x3 x(4,i) x(5,i) x(6,i)];\r
@@ -320,24 +309,24 @@ for i = 1:BUFFER_SIZE
                % update state vector\r
                x(:,i) = x(:,i) + K(:,j) * e(j);\r
                % update covariance matrix\r
-               P = ((eye(6) - (K(:,j) * H(j,:))) * P * (eye(6) - (K(:,j) * H(j,:)))') + (K(:,j) * R(j,j) * K(:,j)');\r
+               P = (eye(6) - (K(:,j) * H(j,:))) * P;\r
        end\r
 end\r
 \r
-euler = Quat2Euler(quat_aid);\r
-roll = euler(:,1)' * RAD;\r
-pitch = euler(:,2)' * RAD;\r
-yaw = euler(:,3)' * RAD;\r
+\r
+roll = roll * RAD2DEG;\r
+pitch = pitch * RAD2DEG;\r
+yaw = yaw * RAD2DEG;\r
 \r
 euler = Quat2Euler(quat_driv);\r
-phi = euler(:,1)' * RAD;\r
-theta = euler(:,2)' * RAD;\r
-psi = -euler(:,3)' * RAD;\r
+phi = euler(:,1)' * RAD2DEG;\r
+theta = euler(:,2)' * RAD2DEG;\r
+psi = -euler(:,3)' * RAD2DEG;\r
 \r
 euler = Quat2Euler(quat_error);\r
-roll_e = euler(:,1)' * RAD;\r
-pitch_e = euler(:,2)' * RAD;\r
-yaw_e = euler(:,3)' * RAD;\r
+roll_e = euler(:,1)' * RAD2DEG;\r
+pitch_e = euler(:,2)' * RAD2DEG;\r
+yaw_e = euler(:,3)' * RAD2DEG;\r
 \r
 % Plot results\r
 close all\r
@@ -498,3 +487,4 @@ grid on;
 p2 = plot(1:BUFFER_SIZE,filt_Mz(1,1:BUFFER_SIZE),'b');\r
 title(['Magnetometer Z-Axis Plot']);\r
 legend([p1 p2],'input signal','low-pass filtered signal');\r
+\r