From 640f8468a27d4500c97f90114963d85394f48752 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Fri, 5 Jun 2015 17:03:36 +0900 Subject: [PATCH] Added code to compute gyro bias in orientation_filter - orientation_filter code will compute gyro bias by adding gyro drift and stocastic noise - null offset bias will be computed and added to gyro bias on individual virtual sensor code. - needed for uncalibrated gyroscope virtual sensor Change-Id: I09cc8af16c12ea77faee704d0ccf6ba0b4d72736 --- src/sensor_fusion/orientation_filter.cpp | 6 ++++++ src/sensor_fusion/orientation_filter.h | 1 + 2 files changed, 7 insertions(+) diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp index c3a6c92..67ff072 100644 --- a/src/sensor_fusion/orientation_filter.cpp +++ b/src/sensor_fusion/orientation_filter.cpp @@ -234,6 +234,8 @@ inline void orientation_filter::time_update() euler_error = (quat2euler(quat_error)).m_ang / (TYPE) PI; + m_gyro_bias = euler_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]); @@ -298,6 +300,8 @@ inline void orientation_filter::time_update_gaming_rv() euler_error = (quat2euler(quat_error)).m_ang / (TYPE) PI; + m_gyro_bias = euler_error.m_ang * (TYPE) PI; + euler_aid = quat2euler(m_quat_aid); euler_driv = quat2euler(quat_output); @@ -351,6 +355,8 @@ inline void orientation_filter::measurement_update() vect vec(arr_bias); m_bias_correction = vec; + + m_gyro_bias = m_gyro_bias + vec; } template diff --git a/src/sensor_fusion/orientation_filter.h b/src/sensor_fusion/orientation_filter.h index c89a6b8..892c87c 100644 --- a/src/sensor_fusion/orientation_filter.h +++ b/src/sensor_fusion/orientation_filter.h @@ -58,6 +58,7 @@ public: vect m_state_old; vect m_state_error; vect m_bias_correction; + vect m_gyro_bias; quaternion m_quat_aid; quaternion m_quat_driv; rotation_matrix m_rot_matrix; -- 2.7.4