From 39814e6c08435919c577de8c7848f5eac2061c65 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Fri, 11 Jul 2014 13:38:33 +0530 Subject: [PATCH] Adding orientation_filter class implementation - complete implementation of the orientation filter class - test application for the orientation filter class signed-off-by: Ramasamy Change-Id: Iee7c01fe8d4dcc2a1f7f71f6f0fb424a2000d989 --- .../standalone/util/orientation_filter.cpp | 293 +++++++++++++++------ .../standalone/util/orientation_filter.h | 49 ++-- .../orientation_filter_main.cpp | 27 +- 3 files changed, 252 insertions(+), 117 deletions(-) diff --git a/src/sensor_fusion/standalone/util/orientation_filter.cpp b/src/sensor_fusion/standalone/util/orientation_filter.cpp index 8fcde52..f3479ce 100644 --- a/src/sensor_fusion/standalone/util/orientation_filter.cpp +++ b/src/sensor_fusion/standalone/util/orientation_filter.cpp @@ -17,34 +17,84 @@ * */ + +#ifdef _ORIENTATION_FILTER_H + #include "orientation_filter.h" #define MOVING_AVERAGE_WINDOW_LENGTH 20 -#define RAD2DEG 57.2957795 -#define DEG2RAD 0.0174532925 -#define US2S (1.0 / 1000000.0) #define GRAVITY 9.80665 -#define PI 3.141593 +#define PI 3.141593 +#define NON_ZERO_VAL 0.1 +#define US2S (1.0 / 1000000.0) +#define SAMPLE_FREQ 100000 // Gyro Types // Systron-donner "Horizon" -#define ZIGMA_W (0.05 * DEG2RAD) //deg/s -#define TAU_W 1000 //secs +#define ZIGMA_W (0.05 * DEG2RAD)//deg/s +#define TAU_W 1000//secs // Crossbow DMU-6X //#define ZIGMA_W 0.05 * DEG2RAD //deg/s //#define TAU_W 300 //secs // FOGs (KVH Autogyro and Crossbow DMU-FOG) //#define ZIGMA_W 0 //deg/s +#define QWB_CONST ((2 * (ZIGMA_W * ZIGMA_W)) / TAU_W) +#define F_CONST (-1 / TAU_W) + +#define BIAS_AX 0.098586 +#define BIAS_AY 0.18385 +#define BIAS_AZ (10.084 - GRAVITY) + +#define BIAS_GX -5.3539 +#define BIAS_GY 0.24325 +#define BIAS_GZ 2.3391 + +#define DRIVING_SYSTEM_PHASE_COMPENSATION -1 + +#define SCALE_GYRO 575 + +#define ENABLE_LPF false + #define M3X3R 3 -#define M3x3C 3 +#define M3X3C 3 + +#define M6X6R 6 +#define M6X6C 6 #define V1x3S 3 +#define V1x4S 4 +#define V1x6S 6 template orientation_filter::orientation_filter() { - + TYPE arr[MOVING_AVERAGE_WINDOW_LENGTH]; + + std::fill_n(arr, MOVING_AVERAGE_WINDOW_LENGTH, NON_ZERO_VAL); + + vector vec(MOVING_AVERAGE_WINDOW_LENGTH, arr); + vector vec1x3(V1x3S); + vector vec1x6(V1x6S); + matrix mat6x6(M6X6R, M6X6C); + + m_var_gyr_x = vec; + m_var_gyr_y = vec; + m_var_gyr_z = vec; + m_var_roll = vec; + m_var_pitch = vec; + m_var_yaw = vec; + + m_tran_mat = mat6x6; + m_measure_mat = mat6x6; + m_pred_cov = mat6x6; + m_driv_cov = mat6x6; + m_aid_cov = mat6x6; + + m_bias_correction = vec1x3; + m_state_new = vec1x6; + m_state_old = vec1x6; + m_state_error = vec1x6; } template @@ -60,42 +110,65 @@ inline void orientation_filter::filter_sensor_data(const sensor_data const TYPE iir_b[] = {0.98, 0}; const TYPE iir_a[] = {1.0000000, 0.02}; - filt_accel[0] = filt_accel[1]; - filt_gyro[0] = filt_gyro[1]; - filt_magnetic[0] = filt_magnetic[1]; + TYPE a_bias[] = {BIAS_AX, BIAS_AY, BIAS_AZ}; + TYPE g_bias[] = {BIAS_GX, BIAS_GY, BIAS_GZ}; + + vector acc_data(V1x3S); + vector gyr_data(V1x3S); + vector acc_bias(V1x3S, a_bias); + vector gyr_bias(V1x3S, g_bias); + + acc_data = accel.m_data - acc_bias; + gyr_data = (gyro.m_data - gyr_bias) / (TYPE) SCALE_GYRO; + + m_filt_accel[0] = m_filt_accel[1]; + m_filt_gyro[0] = m_filt_gyro[1]; + m_filt_magnetic[0] = m_filt_magnetic[1]; - filt_accel[1].m_data = accel.m_data * iir_b[0] - filt_accel[0].m_data * iir_a[1]; - filt_gyro[1].m_data = gyro.m_data * iir_b[0] - filt_gyro[0].m_data * iir_a[1]; - filt_magnetic[1].m_data = magnetic.m_data * iir_b[0] - filt_magnetic[0].m_data * iir_a[1]; + if (ENABLE_LPF) + { + m_filt_accel[1].m_data = acc_data * iir_b[0] - m_filt_accel[0].m_data * iir_a[1]; + m_filt_gyro[1].m_data = gyr_data * iir_b[0] - m_filt_gyro[0].m_data * iir_a[1]; + m_filt_magnetic[1].m_data = magnetic.m_data * iir_b[0] - m_filt_magnetic[0].m_data * iir_a[1]; + } + else + { + m_filt_accel[1].m_data = acc_data; + m_filt_gyro[1].m_data = gyr_data; + m_filt_magnetic[1].m_data = magnetic.m_data; + } - filt_accel[1].m_time_stamp = accel.m_time_stamp; - filt_gyro[1].m_time_stamp = accel.m_time_stamp; - filt_magnetic[1].m_time_stamp = accel.m_time_stamp; + m_filt_accel[1].m_time_stamp = accel.m_time_stamp; + m_filt_gyro[1].m_time_stamp = accel.m_time_stamp; + m_filt_magnetic[1].m_time_stamp = accel.m_time_stamp; + + normalize(m_filt_accel[1]); + normalize(m_filt_magnetic[1]); + + m_filt_gyro[1].m_data = m_filt_gyro[1].m_data - m_bias_correction; } template inline void orientation_filter::orientation_triad_algorithm() { TYPE arr_acc_e[V1x3S] = {0.0, 0.0, 1.0}; - TYPE arr_mag_e[V1x3S] = {0.0, 1.0, 0.0}; + TYPE arr_mag_e[V1x3S] = {0.0, -1.0, 0.0}; - // gravity vector in earth frame vector acc_e(V1x3S, arr_acc_e); - // magnetic field vector in earth frame vector mag_e(V1x3S, arr_mag_e); - vector acc_b_x_mag_b = cross(filt_accel[1].m_data, filt_magnetic[1].m_data); + vector acc_b_x_mag_b = cross(m_filt_accel[1].m_data, m_filt_magnetic[1].m_data); vector acc_e_x_mag_e = cross(acc_e, mag_e); - vector cross1 = cross(acc_b_x_mag_b, filt_accel[1].m_data); + vector cross1 = cross(acc_b_x_mag_b, m_filt_accel[1].m_data); vector cross2 = cross(acc_e_x_mag_e, acc_e); - matrix mat_b(M3X3R, M3x3C); - matrix mat_e(M3X3R, M3x3C); + matrix mat_b(M3X3R, M3X3C); + matrix mat_e(M3X3R, M3X3C); for(int i = 0; i < M3X3R; i++) { - mat_b.m_mat[i][0] = filt_accel[1].m_data.m_vec[i]; + mat_b.m_mat[i][0] = m_filt_accel[1].m_data.m_vec[i]; mat_b.m_mat[i][1] = acc_b_x_mag_b.m_vec[i]; mat_b.m_mat[i][2] = cross1.m_vec[i]; mat_e.m_mat[i][0] = acc_e.m_vec[i]; @@ -103,77 +176,143 @@ inline void orientation_filter::orientation_triad_algorithm() mat_e.m_mat[i][2] = cross2.m_vec[i]; } - matrix mat_b_t = transpose(mat_b); + matrix mat_b_t = tran(mat_b); rotation_matrix rot_mat(mat_e * mat_b_t); - quaternion quat_aid = rot_mat2quat(rot_mat); + + m_quat_aid = rot_mat2quat(rot_mat); } template -inline void orientation_filter::compute_aiding_var() +inline void orientation_filter::compute_covariance() { - + TYPE var_gyr_x, var_gyr_y, var_gyr_z; + TYPE var_roll, var_pitch, var_yaw; + + insert_end(m_var_gyr_x, m_filt_gyro[1].m_data.m_vec[0]); + insert_end(m_var_gyr_y, m_filt_gyro[1].m_data.m_vec[1]); + insert_end(m_var_gyr_z, m_filt_gyro[1].m_data.m_vec[2]); + insert_end(m_var_roll, m_orientation.m_ang.m_vec[0]); + insert_end(m_var_pitch, m_orientation.m_ang.m_vec[1]); + insert_end(m_var_yaw, m_orientation.m_ang.m_vec[2]); + + var_gyr_x = var(m_var_gyr_x); + var_gyr_y = var(m_var_gyr_y); + var_gyr_z = var(m_var_gyr_z); + var_roll = var(m_var_roll); + var_pitch = var(m_var_pitch); + var_yaw = var(m_var_yaw); + + m_driv_cov.m_mat[0][0] = var_gyr_x; + m_driv_cov.m_mat[1][1] = var_gyr_y; + m_driv_cov.m_mat[2][2] = var_gyr_z; + m_driv_cov.m_mat[3][3] = (TYPE) QWB_CONST; + m_driv_cov.m_mat[4][4] = (TYPE) QWB_CONST; + m_driv_cov.m_mat[5][5] = (TYPE) QWB_CONST; + + m_aid_cov.m_mat[0][0] = var_roll; + m_aid_cov.m_mat[1][1] = var_pitch; + m_aid_cov.m_mat[2][2] = var_yaw; } template -inline void orientation_filter::compute_driving_var() +inline void orientation_filter::time_update() { + quaternion quat_diff, quat_error; + euler_angles euler_error; + euler_angles orientation; + unsigned long long sample_interval_gyro = SAMPLE_FREQ; + TYPE dt = 0; -} + if (m_filt_gyro[1].m_time_stamp != 0 && m_filt_gyro[0].m_time_stamp != 0) + sample_interval_gyro = m_filt_gyro[1].m_time_stamp - m_filt_gyro[0].m_time_stamp; -template -inline void orientation_filter::compute_process_covar() -{ + dt = sample_interval_gyro * US2S; -} + m_tran_mat.m_mat[0][1] = m_filt_gyro[1].m_data.m_vec[2]; + m_tran_mat.m_mat[0][2] = -m_filt_gyro[1].m_data.m_vec[1]; + m_tran_mat.m_mat[1][0] = -m_filt_gyro[1].m_data.m_vec[2]; + m_tran_mat.m_mat[1][2] = m_filt_gyro[1].m_data.m_vec[0]; + m_tran_mat.m_mat[2][0] = m_filt_gyro[1].m_data.m_vec[1]; + m_tran_mat.m_mat[2][1] = -m_filt_gyro[1].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; -template -inline void orientation_filter::compute_measurement_covar() -{ + 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))); -template -inline void orientation_filter::compute_prediction_covar() -{ + 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; -template -inline void orientation_filter::compute_quat_diff() -{ + quaternion quat_rot_inc(0, m_filt_gyro[1].m_data.m_vec[0], m_filt_gyro[1].m_data.m_vec[1], + m_filt_gyro[1].m_data.m_vec[2]); -} + quat_diff = (m_quat_driv * quat_rot_inc) * (TYPE) 0.5; -template -inline void orientation_filter::compute_quat_sum() -{ + m_quat_driv = m_quat_driv + (quat_diff * (TYPE) dt * (TYPE) PI); -} + m_quat_driv.quat_normalize(); -template -inline void orientation_filter::update_quat_sum() -{ + orientation = quat2euler(m_quat_driv); -} + m_orientation = orientation.m_ang * (TYPE) DRIVING_SYSTEM_PHASE_COMPENSATION; -template -inline void orientation_filter::time_update() -{ + quat_error = m_quat_aid * m_quat_driv; + + euler_error = (quat2euler(quat_error)).m_ang / (TYPE) PI; + + quaternion quat_eu_er(1, euler_error.m_ang.m_vec[0], euler_error.m_ang.m_vec[1], + euler_error.m_ang.m_vec[2]); + m_quat_driv = (m_quat_driv * quat_eu_er) * (TYPE) PI; + m_quat_driv.quat_normalize(); + + 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(M6X6R, M6X6C); + TYPE iden = 0; -} + for (int j = 0; j < M6X6C; j++) + { + for (int i = 0; i < M6X6R; i++) + { + gain.m_mat[i][j] = m_pred_cov.m_mat[j][i] / (m_pred_cov.m_mat[j][j] + m_aid_cov.m_mat[j][j]); -template -inline euler_angles orientation_filter::get_corrected_orientation() -{ - euler_angles euler_ang; + m_state_new.m_vec[i] = m_state_new.m_vec[i] + gain.m_mat[i][j] * m_state_error.m_vec[j]; + + if (i == j) + iden = 1; + else + iden = 0; + + m_pred_cov.m_mat[i][j] = (iden - (gain.m_mat[i][j] * m_measure_mat.m_mat[j][i])) * m_pred_cov.m_mat[i][j]; + } + } + + m_state_old = m_state_new; + + TYPE arr_bias[V1x3S] = {m_state_new.m_vec[3], m_state_new.m_vec[4], m_state_new.m_vec[5]}; + vector vec(V1x3S, arr_bias); - return euler_ang; + m_bias_correction = vec; } template @@ -184,31 +323,19 @@ euler_angles orientation_filter::get_orientation(const sensor_data class orientation_filter { public: - sensor_data filt_accel[2]; - sensor_data filt_gyro[2]; - sensor_data filt_magnetic[2]; - matrix transition_matrix; - matrix prediction_covariance; - vector state_new; - vector state_old; - vector state_error; - vector bias_correction; - quaternion quat_diff; - quaternion quat_sum; - quaternion quat_aid; - quaternion quat_driv; - quaternion quat_error; - euler_angles euler_error; - rotation_matrix rot_matrix; - euler_angles orientation; + sensor_data m_filt_accel[2]; + sensor_data m_filt_gyro[2]; + sensor_data m_filt_magnetic[2]; + vector m_var_gyr_x; + vector m_var_gyr_y; + vector m_var_gyr_z; + vector m_var_roll; + vector m_var_pitch; + vector m_var_yaw; + matrix m_driv_cov; + matrix m_aid_cov; + matrix m_tran_mat; + matrix m_measure_mat; + matrix m_pred_cov; + vector m_state_new; + vector m_state_old; + vector m_state_error; + vector m_bias_correction; + quaternion m_quat_aid; + quaternion m_quat_driv; + rotation_matrix m_rot_matrix; + euler_angles m_orientation; orientation_filter(); ~orientation_filter(); @@ -54,17 +59,9 @@ public: inline void filter_sensor_data(const sensor_data accel, const sensor_data gyro, const sensor_data magnetic); inline void orientation_triad_algorithm(); - inline void compute_aiding_var(); - inline void compute_driving_var(); - inline void compute_process_covar(); - inline void compute_measurement_covar(); - inline void compute_prediction_covar(); - inline void compute_quat_diff(); - inline void compute_quat_sum(); - inline void update_quat_sum(); + inline void compute_covariance(); inline void time_update(); inline void measurement_update(); - inline euler_angles get_corrected_orientation(); euler_angles get_orientation(const sensor_data accel, const sensor_data gyro, const sensor_data magnetic); diff --git a/src/sensor_fusion/standalone/util/test/orientation_filter_test/orientation_filter_main.cpp b/src/sensor_fusion/standalone/util/test/orientation_filter_test/orientation_filter_main.cpp index 862428d..2743f97 100644 --- a/src/sensor_fusion/standalone/util/test/orientation_filter_test/orientation_filter_main.cpp +++ b/src/sensor_fusion/standalone/util/test/orientation_filter_test/orientation_filter_main.cpp @@ -25,27 +25,30 @@ using namespace std; #define ORIENTATION_DATA_PATH "../../../../design/data/100ms/orientation/roll_pitch_yaw/" -#define ORIENTATION_DATA_SIZE 100 +#define ORIENTATION_DATA_SIZE 1095 int main() { int data_available = ORIENTATION_DATA_SIZE; - ifstream accel_file, gyro_file, magnetic_file; + ifstream accel_in, gyro_in, mag_in; + ofstream orien_file; string line_accel, line_gyro, line_magnetic; float sdata[3]; unsigned long long time_stamp; euler_angles orientation; orientation_filter orien_filter; - accel_file.open(((string)ORIENTATION_DATA_PATH + (string)"accel.txt").c_str()); - gyro_file.open(((string)ORIENTATION_DATA_PATH + (string)"gyro.txt").c_str()); - magnetic_file.open(((string)ORIENTATION_DATA_PATH + (string)"magnetic.txt").c_str()); + accel_in.open(((string)ORIENTATION_DATA_PATH + (string)"accel.txt").c_str()); + gyro_in.open(((string)ORIENTATION_DATA_PATH + (string)"gyro.txt").c_str()); + mag_in.open(((string)ORIENTATION_DATA_PATH + (string)"magnetic.txt").c_str()); + + orien_file.open(((string)"orientation.txt").c_str()); char *token = NULL; while (data_available-- > 0) { - getline(accel_file, line_accel); + getline(accel_in, line_accel); sdata[0] = strtof(line_accel.c_str(), &token); sdata[1] = strtof(token, &token); sdata[2] = strtof(token, &token); @@ -54,7 +57,7 @@ int main() cout << "Accel Data\t" << accel_data.m_data << "\t Time Stamp\t" << accel_data.m_time_stamp << "\n\n"; - getline(gyro_file, line_gyro); + getline(gyro_in, line_gyro); sdata[0] = strtof(line_gyro.c_str(), &token); sdata[1] = strtof(token, &token); sdata[2] = strtof(token, &token); @@ -63,7 +66,7 @@ int main() cout << "Gyro Data\t" << gyro_data.m_data << "\t Time Stamp\t" << gyro_data.m_time_stamp << "\n\n"; - getline(magnetic_file, line_magnetic); + getline(mag_in, line_magnetic); sdata[0] = strtof(line_magnetic.c_str(), &token); sdata[1] = strtof(token, &token); sdata[2] = strtof(token, &token); @@ -74,7 +77,15 @@ int main() orientation = orien_filter.get_orientation(accel_data, gyro_data, magnetic_data); + orien_file << orientation.m_ang; + + cout << "Orientation Data\t" << orientation.m_ang << "\n\n"; } + accel_in.close(); + gyro_in.close(); + mag_in.close(); + orien_file.close(); + return 0; } -- 2.7.4