% 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
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;
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;
quat_driv(1,:) = [1 0 0 0];
end
+ q = [1 0 0 0];
+
% first order filtering
for i = 1:BUFFER_SIZE
% normalize accelerometer measurements
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;
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
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)];
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);
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
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
--- /dev/null
+% 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']);
+
+
}
template <typename TYPE>
+inline void orientation_filter<TYPE>::time_update_gaming_rv()
+{
+ quaternion<TYPE> quat_diff, quat_error, quat_output;
+ euler_angles<TYPE> euler_error;
+ euler_angles<TYPE> orientation;
+ euler_angles<TYPE> euler_aid;
+ euler_angles<TYPE> 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<TYPE> 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<TYPE> 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 <typename TYPE>
inline void orientation_filter<TYPE>::measurement_update()
{
matrix<TYPE, M6X6R, M6X6C> gain;
return m_quat_aid;
}
+
+template <typename TYPE>
+quaternion<TYPE> orientation_filter<TYPE>::get_gaming_quaternion(const sensor_data<TYPE> accel,
+ const sensor_data<TYPE> gyro)
+{
+ euler_angles<TYPE> 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_
projects / platform / core / system / sensord.git / commitdiff