From fe371b4781c76cfd1dd60cda14def2461901463b Mon Sep 17 00:00:00 2001 From: Ankur Date: Tue, 11 Aug 2015 11:43:18 +0530 Subject: [PATCH] Algorithm improvement for orientation based on accel+gyro - Updated algorithm to compensate drift based on mean square threshold values for accelerometer and gyroscope input. - Updated the C-library with improvement in algorithm. Change-Id: I91e99c8722fd568b7da58424b0397e9f07dd456a --- .../design/lib/estimate_orientation.m | 40 +++++++--------------- src/sensor_fusion/orientation_filter.cpp | 19 +++++++--- 2 files changed, 27 insertions(+), 32 deletions(-) diff --git a/src/sensor_fusion/design/lib/estimate_orientation.m b/src/sensor_fusion/design/lib/estimate_orientation.m index de28616..41531f0 100755 --- a/src/sensor_fusion/design/lib/estimate_orientation.m +++ b/src/sensor_fusion/design/lib/estimate_orientation.m @@ -41,8 +41,9 @@ function [quat_driv, quat_aid, quat_error, gyro_bias] = estimate_orientation(Ac GRAVITY = 9.80665; PI = 3.141593; + ACCEL_THRESHOLD = 0.2; + GYRO_THRESHOLD = 0.01; NON_ZERO_VAL = 0.1; - ROUNDOFF_VAL = 0.0025; PI_DEG = 180; MOVING_AVERAGE_WINDOW_LENGTH = 20; @@ -254,10 +255,11 @@ function [quat_driv, quat_aid, quat_error, gyro_bias] = estimate_orientation(Ac q = q / norm(q); else euler_aid = quat2euler(quat_aid(i,:)); - if (euler_aid(1)^2 < (ROUNDOFF_VAL * PI)) - if (euler_aid(2)^2 < (ROUNDOFF_VAL * PI)) - if (gyr_z(i)^2 < (NON_ZERO_VAL)) - q = quat_aid(i,:); + euler_driv = quat2euler(quat_driv(i,:)); + if (acc_y(i)^2 < (ACCEL_THRESHOLD) && Gx(i)^2 < (GYRO_THRESHOLD)) + if (acc_x(i)^2 < (ACCEL_THRESHOLD) && Gy(i)^2 < (GYRO_THRESHOLD)) + if (Gz(i)^2 < (GYRO_THRESHOLD)) + q = euler2quat([euler_aid(2) euler_aid(1) euler_driv(3)]); end end end @@ -341,11 +343,11 @@ function [quat_driv, quat_aid, quat_error, gyro_bias] = estimate_orientation(Ac if GYRO_DATA_DISABLED != 1 p2 = plot(1:BUFFER_SIZE,Gz(1,1:BUFFER_SIZE),'b'); if MAG_DATA_DISABLED != 1 - hold on; - grid on; - p3 = plot(1:BUFFER_SIZE,mag_z(1,1:BUFFER_SIZE),'k'); - title(['Accelerometer/Gyroscope/Magnetometer Z-Axis Plot']); - legend([p1 p2 p3],'Acc_Z', 'Gyr_Z', 'Mag_Z'); + hold on; + grid on; + p3 = plot(1:BUFFER_SIZE,mag_z(1,1:BUFFER_SIZE),'k'); + title(['Accelerometer/Gyroscope/Magnetometer Z-Axis Plot']); + legend([p1 p2 p3],'Acc_Z', 'Gyr_Z', 'Mag_Z'); else title(['Accelerometer/Gyroscope Z-Axis Plot']); legend([p1 p2],'Acc_Z', 'Gyr_Z'); @@ -366,23 +368,17 @@ function [quat_driv, quat_aid, quat_error, gyro_bias] = estimate_orientation(Ac p1 = plot(1:BUFFER_SIZE,Ax(1,1:BUFFER_SIZE),'r'); hold on; grid on; - p2 = plot(1:BUFFER_SIZE,Ax(1,1:BUFFER_SIZE),'b'); title(['Accelerometer X-Axis Plot']); - legend([p1 p2],'input signal','low-pass filtered signal'); subplot(3,1,2) p1 = plot(1:BUFFER_SIZE,Ay(1,1:BUFFER_SIZE),'r'); hold on; grid on; - p2 = plot(1:BUFFER_SIZE,Ay(1,1:BUFFER_SIZE),'b'); title(['Accelerometer Y-Axis Plot']); - legend([p1 p2],'input signal','low-pass filtered signal'); subplot(3,1,3) p1 = plot(1:BUFFER_SIZE,Az(1,1:BUFFER_SIZE),'r'); hold on; grid on; - p2 = plot(1:BUFFER_SIZE,Az(1,1:BUFFER_SIZE),'b'); title(['Accelerometer Z-Axis Plot']); - legend([p1 p2],'input signal','low-pass filtered signal'); if GYRO_DATA_DISABLED != 1 % Gyroscope Raw (vs) filtered output @@ -393,23 +389,17 @@ function [quat_driv, quat_aid, quat_error, gyro_bias] = estimate_orientation(Ac p1 = plot(1:BUFFER_SIZE,Gx(1,1:BUFFER_SIZE),'r'); hold on; grid on; - p2 = plot(1:BUFFER_SIZE,Gx(1,1:BUFFER_SIZE),'b'); title(['Gyroscope X-Axis Plot']); - legend([p1 p2],'input signal','low-pass filtered signal'); subplot(3,1,2) p1 = plot(1:BUFFER_SIZE,Gy(1,1:BUFFER_SIZE),'r'); hold on; grid on; - p2 = plot(1:BUFFER_SIZE,Gy(1,1:BUFFER_SIZE),'b'); title(['Gyroscope Y-Axis Plot']); - legend([p1 p2],'input signal','low-pass filtered signal'); subplot(3,1,3) p1 = plot(1:BUFFER_SIZE,Gz(1,1:BUFFER_SIZE),'r'); hold on; grid on; - p2 = plot(1:BUFFER_SIZE,Gz(1,1:BUFFER_SIZE),'b'); title(['Gyroscope Z-Axis Plot']); - legend([p1 p2],'input signal','low-pass filtered signal'); end if MAG_DATA_DISABLED != 1 @@ -421,23 +411,17 @@ function [quat_driv, quat_aid, quat_error, gyro_bias] = estimate_orientation(Ac p1 = plot(1:BUFFER_SIZE,Mx(1,1:BUFFER_SIZE),'r'); hold on; grid on; - p2 = plot(1:BUFFER_SIZE,Mx(1,1:BUFFER_SIZE),'b'); title(['Magnetometer X-Axis Plot']); - legend([p1 p2],'input signal','low-pass filtered signal'); subplot(3,1,2) p1 = plot(1:BUFFER_SIZE,My(1,1:BUFFER_SIZE),'r'); hold on; grid on; - p2 = plot(1:BUFFER_SIZE,My(1,1:BUFFER_SIZE),'b'); title(['Magnetometer Y-Axis Plot']); - legend([p1 p2],'input signal','low-pass filtered signal'); subplot(3,1,3) p1 = plot(1:BUFFER_SIZE,Mz(1,1:BUFFER_SIZE),'r'); hold on; grid on; - p2 = plot(1:BUFFER_SIZE,Mz(1,1:BUFFER_SIZE),'b'); title(['Magnetometer Z-Axis Plot']); - legend([p1 p2],'input signal','low-pass filtered signal'); end end end diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp index ef2e228..5af46ff 100644 --- a/src/sensor_fusion/orientation_filter.cpp +++ b/src/sensor_fusion/orientation_filter.cpp @@ -33,12 +33,15 @@ #define US2S (1.0 / 1000000.0) //Initialize sampling interval to 100000microseconds #define SAMPLE_INTV 100000 +#define ACCEL_THRESHOLD 0.2 +#define GYRO_THRESHOLD (0.01 * PI) // constants for computation of covariance and transition matrices #define ZIGMA_W (0.05 * DEG2RAD) #define TAU_W 1000 #define QWB_CONST ((2 * (ZIGMA_W * ZIGMA_W)) / TAU_W) #define F_CONST (-1 / TAU_W) +#define SQUARE(T) (T * T) #define NEGLIGIBLE_VAL 0.0000001 @@ -285,10 +288,18 @@ inline void orientation_filter::time_update_gaming_rv() euler_aid = quat2euler(m_quat_aid); euler_driv = quat2euler(m_quat_output); - euler_angles euler_gaming_rv(euler_aid.m_ang.m_vec[0], euler_aid.m_ang.m_vec[1], - euler_driv.m_ang.m_vec[2]); - - m_quat_gaming_rv = euler2quat(euler_gaming_rv); + if ((SQUARE(m_accel.m_data.m_vec[1]) < ACCEL_THRESHOLD) && (SQUARE(m_gyro.m_data.m_vec[0]) < GYRO_THRESHOLD)) + { + if ((SQUARE(m_accel.m_data.m_vec[0]) < ACCEL_THRESHOLD) && (SQUARE(m_gyro.m_data.m_vec[1]) < GYRO_THRESHOLD)) + { + if (SQUARE(m_gyro.m_data.m_vec[2]) < GYRO_THRESHOLD) + { + euler_angles euler_gaming_rv(euler_aid.m_ang.m_vec[0], euler_aid.m_ang.m_vec[1], + euler_driv.m_ang.m_vec[2]); + m_quat_gaming_rv = euler2quat(euler_gaming_rv); + } + } + } if (is_initialized(m_state_new)) { m_state_error.m_vec[0] = m_euler_error.m_ang.m_vec[0]; -- 2.7.4