From 0728c03c5d8ba8a741bd4ff2c7ea22a5ece7bbfb Mon Sep 17 00:00:00 2001 From: Vibhor Gaur Date: Fri, 13 Feb 2015 15:33:50 +0530 Subject: [PATCH] Gaming rotation vector - Feature merge from devel/tizen branch This is third of the four patches being submitted to merge the changes from devel/tizen to tizen branch. This contains the changes for adding gaming rotation vector virtual sensor. Build has been checked for armv7l and aarch64 for public repo. Change-Id: I2da4478c01bf450d49240800695b57969ae22be7 --- src/sensor_fusion/design/lib/estimate_gaming_rv.m | 37 +++ .../design/lib/estimate_orientation.m | 271 +++++++++++++-------- src/sensor_fusion/design/sf_gaming_rv.m | 93 +++++++ src/sensor_fusion/orientation_filter.cpp | 86 +++++++ src/sensor_fusion/orientation_filter.h | 4 + src/sensor_fusion/test/orientation_sensor.cpp | 11 + src/sensor_fusion/test/orientation_sensor.h | 2 + .../orientation_sensor_main.cpp | 7 +- 8 files changed, 408 insertions(+), 103 deletions(-) create mode 100755 src/sensor_fusion/design/lib/estimate_gaming_rv.m create mode 100755 src/sensor_fusion/design/sf_gaming_rv.m diff --git a/src/sensor_fusion/design/lib/estimate_gaming_rv.m b/src/sensor_fusion/design/lib/estimate_gaming_rv.m new file mode 100755 index 0000000..cbaaca7 --- /dev/null +++ b/src/sensor_fusion/design/lib/estimate_gaming_rv.m @@ -0,0 +1,37 @@ +% estimate_gaming_rv +% +% Copyright (c) 2015 Samsung Electronics Co., Ltd. +% +% Licensed under the Apache License, Version 2.0 (the "License"); +% you may not use this file except in compliance with the License. +% You may obtain a copy of the License at +% +% http://www.apache.org/licenses/LICENSE-2.0 +% +% Unless required by applicable law or agreed to in writing, software +% distributed under the License is distributed on an "AS IS" BASIS, +% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +% See the License for the specific language governing permissions and +% limitations under the License. + +% Gaming Rotation Vector Estimation function +% +% - Input orientation using only Accel and Gyroscope sensors +% - Orientation Estimation using estimate_orientation function +% - Output driving system quaternion as gaming rotation vector + + +function [Quat_driv] = estimate_gaming_rv(Accel_data, Gyro_data) + +BUFFER_SIZE = size(Accel_data,2); + +Mag_data = zeros(4,BUFFER_SIZE); + +Quat_driv = zeros(4,BUFFER_SIZE); +Quat_aid = zeros(4,BUFFER_SIZE); +Quat_err = zeros(4,BUFFER_SIZE); + +% estimate orientation +[Quat_driv, Quat_aid, Quat_err] = estimate_orientation(Accel_data, Gyro_data, Mag_data); + +end diff --git a/src/sensor_fusion/design/lib/estimate_orientation.m b/src/sensor_fusion/design/lib/estimate_orientation.m index 60541a5..6f0e852 100755 --- a/src/sensor_fusion/design/lib/estimate_orientation.m +++ b/src/sensor_fusion/design/lib/estimate_orientation.m @@ -16,7 +16,10 @@ % Orientation Estimation function % -% - Orientation Estimation using Gyroscope as the driving system and Accelerometer+Geo-Magnetic Sensors as Aiding System. +% - Orientation and rotation vector Estimation using Gyroscope as the driving system and +% Accelerometer+Geo-Magnetic Sensors as Aiding System. +% - Gaming rotation vector Estimation using Accelerometer and Gyroscope sensors. +% - Geomagnetic rotation vector Estimation using Accelerometer and Geomagnetic sensors. % - Quaternion based approach % - Estimation and correction of orientation errors and bias errors for gyroscope using Kalman filter @@ -26,11 +29,16 @@ function [quat_driv, quat_aid, quat_error] = estimate_orientation(Accel_data, G MAGNETIC_ALIGNMENT_FACTOR = -1; GYRO_DATA_DISABLED = 0; + MAG_DATA_DISABLED = 0; if Gyro_data(4,1) == 0 GYRO_DATA_DISABLED = 1; end + if Mag_data(4,1) == 0 + MAG_DATA_DISABLED = 1; + end + GRAVITY = 9.80665; PI = 3.141593; NON_ZERO_VAL = 0.1; @@ -49,26 +57,31 @@ function [quat_driv, quat_aid, quat_error] = estimate_orientation(Accel_data, G Az = Accel_data(3,:); ATime = Accel_data(4,:); - Mx = Mag_data(1,:); - My = Mag_data(2,:); - Mz = Mag_data(3,:); - MTime = Mag_data(4,:); + mag_x = zeros(1,BUFFER_SIZE); + mag_y = zeros(1,BUFFER_SIZE); + mag_z = zeros(1,BUFFER_SIZE); + MTime = zeros(1,BUFFER_SIZE); + + if MAG_DATA_DISABLED != 1 + Mx = Mag_data(1,:); + My = Mag_data(2,:); + Mz = Mag_data(3,:); + MTime = Mag_data(4,:); + end acc_x = zeros(1,BUFFER_SIZE); acc_y = zeros(1,BUFFER_SIZE); acc_z = zeros(1,BUFFER_SIZE); - mag_x = zeros(1,BUFFER_SIZE); - mag_y = zeros(1,BUFFER_SIZE); - mag_z = zeros(1,BUFFER_SIZE); quat_aid = zeros(BUFFER_SIZE,4); quat_driv = zeros(BUFFER_SIZE,4); + quat_gaming_rv = zeros(BUFFER_SIZE,4); quat_error = zeros(BUFFER_SIZE,4); - + 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 - if GYRO_DATA_DISABLED != 1 + if GYRO_DATA_DISABLED != 1 % Gyroscope Bias Variables Bx = 0; By = 0; Bz = 0; @@ -114,6 +127,8 @@ function [quat_driv, quat_aid, quat_error] = estimate_orientation(Accel_data, G quat_driv(1,:) = [1 0 0 0]; end + q = [1 0 0 0]; + % first order filtering for i = 1:BUFFER_SIZE % normalize accelerometer measurements @@ -122,27 +137,32 @@ function [quat_driv, quat_aid, quat_error] = estimate_orientation(Accel_data, G acc_y(i) = norm_acc * Ay(i); acc_z(i) = norm_acc * Az(i); - % normalize magnetometer measurements - norm_mag = 1/sqrt(Mx(i)^2 + My(i)^2 + Mz(i)^2); - mag_x(i) = norm_mag * Mx(i); - mag_y(i) = norm_mag * My(i); - mag_z(i) = norm_mag * Mz(i); - - UA(i) = sqrt(acc_x(i)^2 + acc_y(i)^2 + acc_z(i)^2) - GRAVITY; - - % Aiding System (Accelerometer + Geomagnetic) quaternion generation % gravity vector in body frame acc_b =[acc_x(i);acc_y(i);acc_z(i)]; - % magnetic field vector in body frame - mag_b =[mag_x(i);mag_y(i);mag_z(i)]; - % compute measurement quaternion with TRIAD algorithm - acc_b_x_mag_b = cross(acc_b,mag_b); - acc_e_x_mag_e = cross(acc_e,mag_e); - M_b = [acc_b acc_b_x_mag_b cross(acc_b_x_mag_b,acc_b)]; - M_e = [acc_e acc_e_x_mag_e cross(acc_e_x_mag_e,acc_e)]; - Rot_m = M_e * M_b'; - quat_aid(i,:) = rot_mat2quat(Rot_m); + if MAG_DATA_DISABLED != 1 + % normalize magnetometer measurements + norm_mag = 1/sqrt(Mx(i)^2 + My(i)^2 + Mz(i)^2); + mag_x(i) = norm_mag * Mx(i); + mag_y(i) = norm_mag * My(i); + mag_z(i) = norm_mag * Mz(i); + + % Aiding System (Accelerometer + Geomagnetic) quaternion generation + % magnetic field vector in body frame + mag_b =[mag_x(i);mag_y(i);mag_z(i)]; + + % compute measurement quaternion with TRIAD algorithm + acc_b_x_mag_b = cross(acc_b,mag_b); + acc_e_x_mag_e = cross(acc_e,mag_e); + M_b = [acc_b acc_b_x_mag_b cross(acc_b_x_mag_b,acc_b)]; + M_e = [acc_e acc_e_x_mag_e cross(acc_e_x_mag_e,acc_e)]; + Rot_m = M_e * M_b'; + quat_aid(i,:) = rot_mat2quat(Rot_m); + else + axis = cross(acc_b, acc_e); + angle = acos(dot(acc_b, acc_e)); + quat_aid(i,:) = axis_rot2quat(axis, angle); + end if GYRO_DATA_DISABLED != 1 gyr_x(i) = Gx(i) * PI; @@ -195,9 +215,7 @@ function [quat_driv, quat_aid, quat_error] = estimate_orientation(Accel_data, G 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))]; + F = [[0 gyr_z(i) -gyr_y(i);-gyr_z(i) 0 gyr_x(i);gyr_y(i) -gyr_x(i) 0] zeros(3,3); zeros(3,3) (-(1/TauW) * eye(3))]; % Time Update if i > 1 @@ -225,8 +243,16 @@ function [quat_driv, quat_aid, quat_error] = estimate_orientation(Accel_data, G 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 MAG_DATA_DISABLED != 1 + q = quat_prod(quat_driv(i,:), [1 x1 x2 x3]) * PI; + q = q / norm(q); + else + euler_aid = quat2euler(quat_aid(i,:)); + euler_driv = quat2euler(quat_driv(i,:)); + + euler_gaming_rv = [euler_aid(2) euler_aid(1) euler_driv(3)]; + quat_gaming_rv(i,:) = euler2quat(euler_gaming_rv); + end if i > 1 e = [x1 x2 x3 x(4,i) x(5,i) x(6,i)]; @@ -247,8 +273,12 @@ function [quat_driv, quat_aid, quat_error] = estimate_orientation(Accel_data, G end end + if MAG_DATA_DISABLED == 1 + quat_driv = quat_gaming_rv; + end + if PLOT_SCALED_SENSOR_COMPARISON_DATA == 1 - % Accelerometer/Gyroscope/Magnetometer scaled Plot results + % Accelerometer/Gyroscope scaled Plot results hfig=(figure); scrsz = get(0,'ScreenSize'); set(hfig,'position',scrsz); @@ -256,32 +286,65 @@ function [quat_driv, quat_aid, quat_error] = estimate_orientation(Accel_data, G p1 = plot(1:BUFFER_SIZE,acc_x(1,1:BUFFER_SIZE),'r'); hold on; grid on; - p2 = plot(1:BUFFER_SIZE,Gx(1,1:BUFFER_SIZE),'b'); - hold on; - grid on; - p3 = plot(1:BUFFER_SIZE,mag_x(1,1:BUFFER_SIZE),'k'); - title(['Accelerometer/Gyroscope/Magnetometer X-Axis Plot']); - legend([p1 p2 p3],'Acc_X', 'Gyr_X', 'Mag_X'); + if GYRO_DATA_DISABLED != 1 + p2 = plot(1:BUFFER_SIZE,Gx(1,1:BUFFER_SIZE),'b'); + if MAG_DATA_DISABLED != 1 + hold on; + grid on; + p3 = plot(1:BUFFER_SIZE,mag_x(1,1:BUFFER_SIZE),'k'); + title(['Accelerometer/Gyroscope/Magnetometer X-Axis Plot']); + legend([p1 p2 p3],'Acc_X', 'Gyr_X', 'Mag_X'); + else + title(['Accelerometer/Gyroscope X-Axis Plot']); + legend([p1 p2],'Acc_X', 'Gyr_X'); + end + else + p2 = plot(1:BUFFER_SIZE,mag_x(1,1:BUFFER_SIZE),'k'); + title(['Accelerometer/Magnetometer X-Axis Plot']); + legend([p1 p2],'Acc_X', 'Mag_X'); + end subplot(3,1,2) p1 = plot(1:BUFFER_SIZE,acc_y(1,1:BUFFER_SIZE),'r'); hold on; grid on; - p2 = plot(1:BUFFER_SIZE,Gy(1,1:BUFFER_SIZE),'b'); - hold on; - grid on; - p3 = plot(1:BUFFER_SIZE,mag_y(1,1:BUFFER_SIZE),'k'); - title(['Accelerometer/Gyroscope/Magnetometer Y-Axis Plot']); - legend([p1 p2 p3],'Acc_Y', 'Gyr_Y', 'Mag_Y'); + if GYRO_DATA_DISABLED != 1 + p2 = plot(1:BUFFER_SIZE,Gy(1,1:BUFFER_SIZE),'b'); + if MAG_DATA_DISABLED != 1 + hold on; + grid on; + p3 = plot(1:BUFFER_SIZE,mag_y(1,1:BUFFER_SIZE),'k'); + title(['Accelerometer/Gyroscope/Magnetometer Y-Axis Plot']); + legend([p1 p2 p3],'Acc_Y', 'Gyr_Y', 'Mag_Y'); + else + title(['Accelerometer/Gyroscope Y-Axis Plot']); + legend([p1 p2],'Acc_Y', 'Gyr_Y'); + end + else + p2 = plot(1:BUFFER_SIZE,mag_y(1,1:BUFFER_SIZE),'k'); + title(['Accelerometer/Magnetometer Y-Axis Plot']); + legend([p1 p2],'Acc_X', 'Mag_Y'); + end subplot(3,1,3) p1 = plot(1:BUFFER_SIZE,acc_z(1,1:BUFFER_SIZE),'r'); hold on; grid on; - p2 = plot(1:BUFFER_SIZE,Gz(1,1:BUFFER_SIZE),'b'); - 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'); + 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'); + else + title(['Accelerometer/Gyroscope Z-Axis Plot']); + legend([p1 p2],'Acc_Z', 'Gyr_Z'); + end + else + p2 = plot(1:BUFFER_SIZE,mag_z(1,1:BUFFER_SIZE),'k'); + title(['Accelerometer/Magnetometer Z-Axis Plot']); + legend([p1 p2],'Acc_Z', 'Mag_Z'); + end end if PLOT_INDIVIDUAL_SENSOR_INPUT_DATA == 1 @@ -311,56 +374,60 @@ function [quat_driv, quat_aid, quat_error] = estimate_orientation(Accel_data, G title(['Accelerometer Z-Axis Plot']); legend([p1 p2],'input signal','low-pass filtered signal'); - % Gyroscope Raw (vs) filtered output - hfig=(figure); - scrsz = get(0,'ScreenSize'); - set(hfig,'position',scrsz); - subplot(3,1,1) - 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'); + if GYRO_DATA_DISABLED != 1 + % Gyroscope Raw (vs) filtered output + hfig=(figure); + scrsz = get(0,'ScreenSize'); + set(hfig,'position',scrsz); + subplot(3,1,1) + 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 - % Magnetometer Raw (vs) filtered output - hfig=(figure); - scrsz = get(0,'ScreenSize'); - set(hfig,'position',scrsz); - subplot(3,1,1) - 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'); + if MAG_DATA_DISABLED != 1 + % Magnetometer Raw (vs) filtered output + hfig=(figure); + scrsz = get(0,'ScreenSize'); + set(hfig,'position',scrsz); + subplot(3,1,1) + 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/design/sf_gaming_rv.m b/src/sensor_fusion/design/sf_gaming_rv.m new file mode 100755 index 0000000..892b86e --- /dev/null +++ b/src/sensor_fusion/design/sf_gaming_rv.m @@ -0,0 +1,93 @@ +% sf_gaming_rv +% +% Copyright (c) 2015 Samsung Electronics Co., Ltd. +% +% Licensed under the Apache License, Version 2.0 (the "License"); +% you may not use this file except in compliance with the License. +% You may obtain a copy of the License at +% +% http://www.apache.org/licenses/LICENSE-2.0 +% +% Unless required by applicable law or agreed to in writing, software +% distributed under the License is distributed on an "AS IS" BASIS, +% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +% See the License for the specific language governing permissions and +% limitations under the License. + +% Sensor Fusion Implementation for Determination of Gaming rotation Vector +% +% - Input Accelerometer and Gyroscope sensor data +% - Call estimate_gaming_rotation +% - Plot results for gaming rotation on reference axis + +addpath('lib'); +clear +close all +clc + +GRAVITY = 9.80665; + +Max_Range_Accel = 39.203407; Min_Range_Accel = -39.204006; Res_Accel = 0.000598; +Max_Range_Gyro = 1146.862549; Min_Range_Gyro = -1146.880005; Res_Gyro = 0.017500; + +Bias_Ax = 0.098586; +Bias_Ay = 0.18385; +Bias_Az = 10.084 - GRAVITY; + +Bias_Gx = -5.3539; +Bias_Gy = 0.24325; +Bias_Gz = 2.3391; + +BUFFER_SIZE = 1095; + +Accel_data = zeros(4,BUFFER_SIZE); +Gyro_data = zeros(4,BUFFER_SIZE); + +Game_RV = zeros(4,BUFFER_SIZE); +Orientation_RV = zeros(3,BUFFER_SIZE); + +% Sensor Data simulating orientation motions + +% get accel x,y,z axis data from stored file +Accel_data(1,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/accel.txt")(:,1))') - Bias_Ax)(1:BUFFER_SIZE); +Accel_data(2,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/accel.txt")(:,2))') - Bias_Ay)(1:BUFFER_SIZE); +Accel_data(3,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/accel.txt")(:,3))') - Bias_Az)(1:BUFFER_SIZE); +Accel_data(4,:) = ((dlmread("data/100ms/orientation/roll_pitch_yaw/accel.txt")(:,4))')(1:BUFFER_SIZE); + +% get gyro x,y,z axis data from stored file +Gyro_data(1,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/gyro.txt")(:,1))') - Bias_Gx)(1:BUFFER_SIZE); +Gyro_data(2,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/gyro.txt")(:,2))') - Bias_Gy)(1:BUFFER_SIZE); +Gyro_data(3,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/gyro.txt")(:,3))') - Bias_Gz)(1:BUFFER_SIZE); +Gyro_data(4,:) = ((dlmread("data/100ms/orientation/roll_pitch_yaw/gyro.txt")(:,4))')(1:BUFFER_SIZE); + +scale_Gyro = 575; +Gyro_data(1,:) = Gyro_data(1,:)/scale_Gyro; +Gyro_data(2,:) = Gyro_data(2,:)/scale_Gyro; +Gyro_data(3,:) = Gyro_data(3,:)/scale_Gyro; + +% estimate orientation +Game_RV = estimate_gaming_rv(Accel_data, Gyro_data); + +for i = 1:BUFFER_SIZE + Orientation_RV(:,i) = quat2euler(Game_RV(i,:)); +end + +hfig=(figure); +scrsz = get(0,'ScreenSize'); +set(hfig,'position',scrsz); +% Gaming Rotation Vector Plot Results +subplot(3,1,1) +UA = Orientation_RV(1,:); +p1 = plot(1:length(UA),UA(1,1:length(UA)),'k'); +legend(p1,'x-axis'); +subplot(3,1,2) +UA = Orientation_RV(2,:); +p1 = plot(1:length(UA),UA(1,1:length(UA)),'b'); +legend(p1,'y-axis'); +subplot(3,1,3) +UA = Orientation_RV(3,:); +p1 = plot(1:length(UA),UA(1,1:length(UA)),'r'); +legend(p1,'z-axis'); +title(['Gaming Rotation Vector']); + + diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp index d9e353f..470946d 100644 --- a/src/sensor_fusion/orientation_filter.cpp +++ b/src/sensor_fusion/orientation_filter.cpp @@ -272,6 +272,70 @@ inline void orientation_filter::time_update() } template +inline void orientation_filter::time_update_gaming_rv() +{ + quaternion quat_diff, quat_error, quat_output; + euler_angles euler_error; + euler_angles orientation; + euler_angles euler_aid; + euler_angles euler_driv; + + m_tran_mat.m_mat[0][1] = m_gyro.m_data.m_vec[2]; + m_tran_mat.m_mat[0][2] = -m_gyro.m_data.m_vec[1]; + m_tran_mat.m_mat[1][0] = -m_gyro.m_data.m_vec[2]; + m_tran_mat.m_mat[1][2] = m_gyro.m_data.m_vec[0]; + m_tran_mat.m_mat[2][0] = m_gyro.m_data.m_vec[1]; + m_tran_mat.m_mat[2][1] = -m_gyro.m_data.m_vec[0]; + m_tran_mat.m_mat[3][3] = (TYPE) F_CONST; + m_tran_mat.m_mat[4][4] = (TYPE) F_CONST; + m_tran_mat.m_mat[5][5] = (TYPE) F_CONST; + + m_measure_mat.m_mat[0][0] = 1; + m_measure_mat.m_mat[1][1] = 1; + m_measure_mat.m_mat[2][2] = 1; + + if (is_initialized(m_state_old)) + m_state_new = transpose(mul(m_tran_mat, transpose(m_state_old))); + + m_pred_cov = (m_tran_mat * m_pred_cov * tran(m_tran_mat)) + m_driv_cov; + + if(!is_initialized(m_quat_driv.m_quat)) + m_quat_driv = m_quat_aid; + + quaternion quat_rot_inc(0, m_gyro.m_data.m_vec[0], m_gyro.m_data.m_vec[1], + m_gyro.m_data.m_vec[2]); + + quat_diff = (m_quat_driv * quat_rot_inc) * (TYPE) 0.5; + + m_quat_driv = m_quat_driv + (quat_diff * (TYPE) m_gyro_dt * (TYPE) PI); + m_quat_driv.quat_normalize(); + + quat_output = phase_correction(m_quat_driv, m_quat_aid); + + quat_error = m_quat_aid * m_quat_driv; + + euler_error = (quat2euler(quat_error)).m_ang / (TYPE) PI; + + euler_aid = quat2euler(m_quat_aid); + euler_driv = quat2euler(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 (is_initialized(m_state_new)) + { + m_state_error.m_vec[0] = euler_error.m_ang.m_vec[0]; + m_state_error.m_vec[1] = euler_error.m_ang.m_vec[1]; + m_state_error.m_vec[2] = euler_error.m_ang.m_vec[2]; + m_state_error.m_vec[3] = m_state_new.m_vec[3]; + m_state_error.m_vec[4] = m_state_new.m_vec[4]; + m_state_error.m_vec[5] = m_state_new.m_vec[5]; + } +} + +template inline void orientation_filter::measurement_update() { matrix gain; @@ -359,4 +423,26 @@ quaternion orientation_filter::get_geomagnetic_quaternion(const sens return m_quat_aid; } + +template +quaternion orientation_filter::get_gaming_quaternion(const sensor_data accel, + const sensor_data gyro) +{ + euler_angles cor_euler_ang; + + init_accel_gyro_data(accel, gyro); + + normalize(m_accel); + m_gyro.m_data = m_gyro.m_data * (TYPE) PI; + + compute_accel_orientation(); + + compute_covariance(); + + time_update_gaming_rv(); + + measurement_update(); + + return m_quat_gaming_rv; +} #endif //_ORIENTATION_FILTER_H_ diff --git a/src/sensor_fusion/orientation_filter.h b/src/sensor_fusion/orientation_filter.h index bf89c96..a04f3f7 100644 --- a/src/sensor_fusion/orientation_filter.h +++ b/src/sensor_fusion/orientation_filter.h @@ -63,6 +63,7 @@ public: rotation_matrix m_rot_matrix; euler_angles m_orientation; quaternion m_quat_9axis; + quaternion m_quat_gaming_rv; TYPE m_gyro_dt; int m_pitch_phase_compensation; @@ -83,6 +84,7 @@ public: inline void compute_accel_orientation(); inline void compute_covariance(); inline void time_update(); + inline void time_update_gaming_rv(); inline void measurement_update(); euler_angles get_orientation(const sensor_data accel, @@ -93,6 +95,8 @@ public: const sensor_data gyro, const sensor_data magnetic); quaternion get_geomagnetic_quaternion(const sensor_data accel, const sensor_data magnetic); + quaternion get_gaming_quaternion(const sensor_data accel, + const sensor_data gyro); }; #include "orientation_filter.cpp" diff --git a/src/sensor_fusion/test/orientation_sensor.cpp b/src/sensor_fusion/test/orientation_sensor.cpp index 06ebf5a..70746f5 100644 --- a/src/sensor_fusion/test/orientation_sensor.cpp +++ b/src/sensor_fusion/test/orientation_sensor.cpp @@ -105,4 +105,15 @@ quaternion orientation_sensor::get_geomagnetic_quaternion(sensor_data orientation_sensor::get_gaming_quaternion(sensor_data accel_data, + sensor_data gyro_data) +{ + pre_process_data(accel_data, accel_data, bias_accel, sign_accel, scale_accel); + normalize(accel_data); + pre_process_data(gyro_data, gyro_data, bias_gyro, sign_gyro, scale_gyro); + + return orien_filter.get_gaming_quaternion(accel_data, gyro_data); +} #endif diff --git a/src/sensor_fusion/test/orientation_sensor.h b/src/sensor_fusion/test/orientation_sensor.h index e9fd2b8..358c476 100644 --- a/src/sensor_fusion/test/orientation_sensor.h +++ b/src/sensor_fusion/test/orientation_sensor.h @@ -35,6 +35,8 @@ public: sensor_data gyro, sensor_data magnetic); quaternion get_geomagnetic_quaternion(sensor_data accel, sensor_data magnetic); + quaternion get_gaming_quaternion(sensor_data accel, + sensor_data gyro); }; #include "orientation_sensor.cpp" diff --git a/src/sensor_fusion/test/test_projects/orientation_sensor_test/orientation_sensor_main.cpp b/src/sensor_fusion/test/test_projects/orientation_sensor_test/orientation_sensor_main.cpp index a30f8ee..c5593e9 100644 --- a/src/sensor_fusion/test/test_projects/orientation_sensor_test/orientation_sensor_main.cpp +++ b/src/sensor_fusion/test/test_projects/orientation_sensor_test/orientation_sensor_main.cpp @@ -39,7 +39,8 @@ int main() rotation_matrix orientation_mat; quaternion orientation_9axis_quat; quaternion orientation_geomagnetic_quat; - orientation_sensor orien_sensor1, orien_sensor2, orien_sensor3, orien_sensor4; + quaternion orientation_gaming_quat; + orientation_sensor orien_sensor1, orien_sensor2, orien_sensor3, orien_sensor4, orien_sensor5; accel_in.open(((string)ORIENTATION_DATA_PATH + (string)"accel.txt").c_str()); gyro_in.open(((string)ORIENTATION_DATA_PATH + (string)"gyro.txt").c_str()); @@ -95,6 +96,10 @@ int main() orientation_geomagnetic_quat = orien_sensor4.get_geomagnetic_quaternion(accel_data, magnetic_data); cout << "Orientation geomagnetic quaternion\t" << orientation_geomagnetic_quat.m_quat << "\n\n"; + + orientation_gaming_quat = orien_sensor5.get_gaming_quaternion(accel_data, gyro_data); + + cout << "Orientation gaming quaternion\t" << orientation_gaming_quat.m_quat << "\n\n"; } accel_in.close(); -- 2.7.4