From c11bf0cd00080a1a74a7bc28a3dbb1db0d6aa92e Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Wed, 7 Jan 2015 10:06:16 +0530 Subject: [PATCH] Adapting estimate_orientation to support geomagnetic_RV Restructuring estimate_orientation octave code to support both quaternion generation using accel/gyro/mag or only using accel/mag hardware sensors. Change-Id: I0976df35d488d6bafb9ce064027ff7fd6e352dbd --- .../design/lib/estimate_orientation.m | 305 +++++++++++---------- 1 file changed, 155 insertions(+), 150 deletions(-) diff --git a/src/sensor_fusion/design/lib/estimate_orientation.m b/src/sensor_fusion/design/lib/estimate_orientation.m index 187ef38..0094b04 100755 --- a/src/sensor_fusion/design/lib/estimate_orientation.m +++ b/src/sensor_fusion/design/lib/estimate_orientation.m @@ -25,6 +25,11 @@ function [quat_driv, quat_aid, quat_error] = estimate_orientation(Accel_data, G PLOT_INDIVIDUAL_SENSOR_INPUT_DATA = 0; MAGNETIC_ALIGNMENT_FACTOR = -1; + GYRO_DATA_DISABLED = 0; + + if Gyro_data(4,1) == 0 + GYRO_DATA_DISABLED = 1; + end GRAVITY = 9.80665; PI = 3.141593; @@ -35,15 +40,8 @@ function [quat_driv, quat_aid, quat_error] = estimate_orientation(Accel_data, G DEG2RAD = 0.0174532925; US2S = 1.0 / 1000000.0; - % Gyro Types - % Systron-donner "Horizon" - ZigmaW = 0.05 * DEG2RAD; %deg/s - TauW = 1000; %secs - % Crossbow DMU-6X - %ZigmaW = 0.05 * DEG2RAD; %deg/s - %TauW = 300; %secs - %FOGs (KVH Autogyro and Crossbow DMU-FOG) - %ZigmaW = 0; %deg/s + ZigmaW = 0.05 * DEG2RAD; + TauW = 1000; BUFFER_SIZE = size(Accel_data,2); Ax = Accel_data(1,:); @@ -51,66 +49,70 @@ function [quat_driv, quat_aid, quat_error] = estimate_orientation(Accel_data, G Az = Accel_data(3,:); ATime = Accel_data(4,:); - Gx = Gyro_data(1,:); - Gy = Gyro_data(2,:); - Gz = Gyro_data(3,:); - GTime = Gyro_data(4,:); - Mx = Mag_data(1,:); My = Mag_data(2,:); Mz = Mag_data(3,:); MTime = Mag_data(4,:); - % Gyroscope Bias Variables - Bx = 0; By = 0; Bz = 0; - acc_x = zeros(1,BUFFER_SIZE); acc_y = zeros(1,BUFFER_SIZE); acc_z = zeros(1,BUFFER_SIZE); - gyr_x = zeros(1,BUFFER_SIZE); - gyr_y = zeros(1,BUFFER_SIZE); - gyr_z = zeros(1,BUFFER_SIZE); mag_x = zeros(1,BUFFER_SIZE); mag_y = zeros(1,BUFFER_SIZE); mag_z = zeros(1,BUFFER_SIZE); - % User Acceleration mean and Variance - A_T = zeros(1,BUFFER_SIZE); - G_T = zeros(1,BUFFER_SIZE); - M_T = zeros(1,BUFFER_SIZE); - var_roll = zeros(1,BUFFER_SIZE); - var_pitch = zeros(1,BUFFER_SIZE); - var_yaw = zeros(1,BUFFER_SIZE); - var_Gx = zeros(1,BUFFER_SIZE); - var_Gy = zeros(1,BUFFER_SIZE); - var_Gz = zeros(1,BUFFER_SIZE); - - roll = zeros(1,BUFFER_SIZE); - pitch = zeros(1,BUFFER_SIZE); - yaw = zeros(1,BUFFER_SIZE); - quat_driv = zeros(BUFFER_SIZE,4); quat_aid = zeros(BUFFER_SIZE,4); + quat_driv = zeros(BUFFER_SIZE,4); quat_error = zeros(BUFFER_SIZE,4); - - % system covariance matrix - Q = zeros(6,6); - - % measurement covariance matrix - R = zeros(6,6); - - A_T(1) = 100000; - G_T(1) = 100000; - M_T(1) = 100000; - + acc_e = [0.0;0.0;1.0]; % gravity 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); - e = zeros(1,6); - P = 1 * eye(6);% state covariance matrix - - quat_driv(1,:) = [1 0 0 0]; + if GYRO_DATA_DISABLED != 1 + % Gyroscope Bias Variables + Bx = 0; By = 0; Bz = 0; + + Gx = Gyro_data(1,:); + Gy = Gyro_data(2,:); + Gz = Gyro_data(3,:); + GTime = Gyro_data(4,:); + + gyr_x = zeros(1,BUFFER_SIZE); + gyr_y = zeros(1,BUFFER_SIZE); + gyr_z = zeros(1,BUFFER_SIZE); + + % User Acceleration mean and Variance + A_T = zeros(1,BUFFER_SIZE); + G_T = zeros(1,BUFFER_SIZE); + M_T = zeros(1,BUFFER_SIZE); + var_roll = zeros(1,BUFFER_SIZE); + var_pitch = zeros(1,BUFFER_SIZE); + var_yaw = zeros(1,BUFFER_SIZE); + var_Gx = zeros(1,BUFFER_SIZE); + var_Gy = zeros(1,BUFFER_SIZE); + var_Gz = zeros(1,BUFFER_SIZE); + + roll = zeros(1,BUFFER_SIZE); + pitch = zeros(1,BUFFER_SIZE); + yaw = zeros(1,BUFFER_SIZE); + + % system covariance matrix + Q = zeros(6,6); + + % measurement covariance matrix + R = zeros(6,6); + + A_T(1) = 100000; + G_T(1) = 100000; + M_T(1) = 100000; + + H = [eye(3) zeros(3,3); zeros(3,6)]; + x = zeros(6,BUFFER_SIZE); + e = zeros(1,6); + P = 1 * eye(6);% state covariance matrix + + quat_driv(1,:) = [1 0 0 0]; + end % first order filtering for i = 1:BUFFER_SIZE @@ -126,16 +128,8 @@ function [quat_driv, quat_aid, quat_error] = estimate_orientation(Accel_data, G mag_y(i) = norm_mag * My(i); mag_z(i) = norm_mag * Mz(i); - gyr_x(i) = Gx(i) * PI; - gyr_y(i) = Gy(i) * PI; - gyr_z(i) = Gz(i) * PI; - UA(i) = sqrt(acc_x(i)^2 + acc_y(i)^2 + acc_z(i)^2) - GRAVITY; - gyr_x(i) = gyr_x(i) - Bx; - gyr_y(i) = gyr_y(i) - By; - gyr_z(i) = gyr_z(i) - Bz; - % Aiding System (Accelerometer + Geomagnetic) quaternion generation % gravity vector in body frame acc_b =[acc_x(i);acc_y(i);acc_z(i)]; @@ -150,98 +144,109 @@ function [quat_driv, quat_aid, quat_error] = estimate_orientation(Accel_data, G Rot_m = M_e * M_b'; quat_aid(i,:) = rot_mat2quat(Rot_m); - euler = quat2euler(quat_aid(i,:)); - roll(i) = euler(2); - pitch(i) = euler(1); - yaw(i) = euler(3); - - if i <= MOVING_AVERAGE_WINDOW_LENGTH - var_Gx(i) = NON_ZERO_VAL; - var_Gy(i) = NON_ZERO_VAL; - var_Gz(i) = NON_ZERO_VAL; - var_roll(i) = NON_ZERO_VAL; - var_pitch(i) = NON_ZERO_VAL; - var_yaw(i) = NON_ZERO_VAL; - else - var_Gx(i) = var(gyr_x((i-MOVING_AVERAGE_WINDOW_LENGTH):i)); - var_Gy(i) = var(gyr_y((i-MOVING_AVERAGE_WINDOW_LENGTH):i)); - var_Gz(i) = var(gyr_z((i-MOVING_AVERAGE_WINDOW_LENGTH):i)); - var_roll(i) = var(roll((i-MOVING_AVERAGE_WINDOW_LENGTH):i)); - var_pitch(i) = var(pitch((i-MOVING_AVERAGE_WINDOW_LENGTH):i)); - var_yaw(i) = var(yaw((i-MOVING_AVERAGE_WINDOW_LENGTH):i)); - end - if i > 1 - A_T(i) = ATime(i) - ATime(i-1); - G_T(i) = GTime(i) - GTime(i-1); - M_T(i) = MTime(i) - MTime(i-1); + if GYRO_DATA_DISABLED != 1 + gyr_x(i) = Gx(i) * PI; + gyr_y(i) = Gy(i) * PI; + gyr_z(i) = Gz(i) * PI; + + gyr_x(i) = gyr_x(i) - Bx; + gyr_y(i) = gyr_y(i) - By; + gyr_z(i) = gyr_z(i) - Bz; + + euler = quat2euler(quat_aid(i,:)); + roll(i) = euler(2); + pitch(i) = euler(1); + yaw(i) = euler(3); + + if i <= MOVING_AVERAGE_WINDOW_LENGTH + var_Gx(i) = NON_ZERO_VAL; + var_Gy(i) = NON_ZERO_VAL; + var_Gz(i) = NON_ZERO_VAL; + var_roll(i) = NON_ZERO_VAL; + var_pitch(i) = NON_ZERO_VAL; + var_yaw(i) = NON_ZERO_VAL; + else + var_Gx(i) = var(gyr_x((i-MOVING_AVERAGE_WINDOW_LENGTH):i)); + var_Gy(i) = var(gyr_y((i-MOVING_AVERAGE_WINDOW_LENGTH):i)); + var_Gz(i) = var(gyr_z((i-MOVING_AVERAGE_WINDOW_LENGTH):i)); + var_roll(i) = var(roll((i-MOVING_AVERAGE_WINDOW_LENGTH):i)); + var_pitch(i) = var(pitch((i-MOVING_AVERAGE_WINDOW_LENGTH):i)); + var_yaw(i) = var(yaw((i-MOVING_AVERAGE_WINDOW_LENGTH):i)); + end + if i > 1 + A_T(i) = ATime(i) - ATime(i-1); + G_T(i) = GTime(i) - GTime(i-1); + M_T(i) = MTime(i) - MTime(i-1); + end + + dt = G_T(i) * US2S; + + Qwn = [var_Gx(i) 0 0;0 var_Gy(i) 0;0 0 var_Gz(i);]; + Qwb = (2 * (ZigmaW^2) / TauW) * eye(3); + + % Process Noise Covariance + Q = [Qwn zeros(3,3);zeros(3,3) Qwb]; + + % Measurement Noise Covariance + 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)]; + + % initialization for q + if i == 1 + q = quat_aid(i,:); + end + + q_t = [q(2) q(3) q(4)]'; + 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]; + 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))]; + + % Time Update + if i > 1 + x(:,i) = F * x(:,i-1); + end + + % compute covariance of prediction + P = (F * P * F') + Q; + + % Driving System (Gyroscope) quaternion generation + % convert scaled gyro data to rad/s + qDot = 0.5 * quat_prod(q, [0 gyr_x(i) gyr_y(i) gyr_z(i)]); + + % Integrate to yield quaternion + q = q + qDot * dt * PI; + + % normalise quaternion + quat_driv(i,:) = q / norm(q); + + % Kalman Filtering + quat_error(i,:) = quat_prod(quat_aid(i,:), quat_driv(i,:)); + + euler_e = quat2euler(quat_error(i,:)); + x1 = euler_e(1)'/PI; + x2 = euler_e(2)'/PI; + x3 = euler_e(3)'/PI; + + q = quat_prod(quat_driv(i,:), [1 x1 x2 x3]) * PI; + q = q / norm(q); + + if i > 1 + e = [x1 x2 x3 x(4,i) x(5,i) x(6,i)]; + end + + for j =1:6 + % compute Kalman gain + K(:,j) = P(j ,:)./(P(j,j)+R(j,j)); + % update state vector + x(:,i) = x(:,i) + K(:,j) * e(j); + % update covariance matrix + P = (eye(6) - (K(:,j) * H(j,:))) * P; + end + + Bx = x(4,i); + By = x(5,i); + Bz = x(6,i); end - - dt = G_T(i) * US2S; - - Qwn = [var_Gx(i) 0 0;0 var_Gy(i) 0;0 0 var_Gz(i);]; - Qwb = (2 * (ZigmaW^2) / TauW) * eye(3); - - % Process Noise Covariance - Q = [Qwn zeros(3,3);zeros(3,3) Qwb]; - - % Measurement Noise Covariance - 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)]; - - % initialization for q - if i == 1 - q = quat_aid(i,:); - end - - q_t = [q(2) q(3) q(4)]'; - 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]; - 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))]; - - % Time Update - if i > 1 - x(:,i) = F * x(:,i-1); - end - - % compute covariance of prediction - P = (F * P * F') + Q; - - % Driving System (Gyroscope) quaternion generation - % convert scaled gyro data to rad/s - qDot = 0.5 * quat_prod(q, [0 gyr_x(i) gyr_y(i) gyr_z(i)]); - - % Integrate to yield quaternion - q = q + qDot * dt * PI; - - % normalise quaternion - quat_driv(i,:) = q / norm(q); - - % Kalman Filtering - quat_error(i,:) = quat_prod(quat_aid(i,:), quat_driv(i,:)); - - euler_e = quat2euler(quat_error(i,:)); - x1 = euler_e(1)'/PI; - x2 = euler_e(2)'/PI; - x3 = euler_e(3)'/PI; - - q = quat_prod(quat_driv(i,:), [1 x1 x2 x3]) * PI; - q = q / norm(q); - - if i > 1 - e = [x1 x2 x3 x(4,i) x(5,i) x(6,i)]; - end - - for j =1:6 - % compute Kalman gain - K(:,j) = P(j ,:)./(P(j,j)+R(j,j)); - % update state vector - x(:,i) = x(:,i) + K(:,j) * e(j); - % update covariance matrix - P = (eye(6) - (K(:,j) * H(j,:))) * P; - end - - Bx = x(4,i); - By = x(5,i); - Bz = x(6,i); end + if PLOT_SCALED_SENSOR_COMPARISON_DATA == 1 % Accelerometer/Gyroscope/Magnetometer scaled Plot results hfig=(figure); -- 2.7.4