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

diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp
index c3a6c92..67ff072 100644 (file)
--- a/src/sensor_fusion/orientation_filter.cpp
+++ b/src/sensor_fusion/orientation_filter.cpp
@@ -234,6 +234,8 @@ inline void orientation_filter<TYPE>::time_update()
 
        euler_error = (quat2euler(quat_error)).m_ang / (TYPE) PI;
 
+       m_gyro_bias = euler_error.m_ang * (TYPE) PI;
+
        quaternion<TYPE> 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<TYPE>::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<TYPE>::measurement_update()
        vect<TYPE, V1x3S> vec(arr_bias);
 
        m_bias_correction = vec;
+
+       m_gyro_bias = m_gyro_bias + vec;
 }
 
 template <typename TYPE>

diff --git a/src/sensor_fusion/orientation_filter.h b/src/sensor_fusion/orientation_filter.h
index c89a6b8..892c87c 100644 (file)
--- a/src/sensor_fusion/orientation_filter.h
+++ b/src/sensor_fusion/orientation_filter.h
@@ -58,6 +58,7 @@ public:
        vect<TYPE, V1x6S> m_state_old;
        vect<TYPE, V1x6S> m_state_error;
        vect<TYPE, V1x3S> m_bias_correction;
+       vect<TYPE, V1x3S> m_gyro_bias;
        quaternion<TYPE> m_quat_aid;
        quaternion<TYPE> m_quat_driv;
        rotation_matrix<TYPE> m_rot_matrix;