From 0b5c90574c78f683294c719a40b66adb846d77af Mon Sep 17 00:00:00 2001 From: Roman Date: Thu, 14 Jan 2016 16:59:04 +0100 Subject: [PATCH] fix delta angle bias usage: the delta angle bias was applied to imu data which was not coming in the same time intervall as the filter was operating. Therefore, the delta angle bias applied to new imu data had to be scaled correctly in order to match the imu time interval. --- EKF/ekf.cpp | 14 ++++++-------- EKF/estimator_base.cpp | 9 +++++---- 2 files changed, 11 insertions(+), 12 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 7d666873dd..547d27baf0 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -232,14 +232,14 @@ void Ekf::calculateOutputStates() { imuSample imu_new = _imu_sample_new; Vector3f delta_angle; - delta_angle(0) = imu_new.delta_ang(0) * _state.gyro_scale(0); - delta_angle(1) = imu_new.delta_ang(1) * _state.gyro_scale(1); - delta_angle(2) = imu_new.delta_ang(2) * _state.gyro_scale(2); - delta_angle -= _state.gyro_bias; + // Note: We do no not need to consider any bias or scale correction here + // since the base class has already corrected the imu sample + delta_angle(0) = imu_new.delta_ang(0); + delta_angle(1) = imu_new.delta_ang(1); + delta_angle(2) = imu_new.delta_ang(2); Vector3f delta_vel = imu_new.delta_vel; - delta_vel(2) -= _state.accel_z_bias; delta_angle += _delta_angle_corr; Quaternion dq; @@ -265,9 +265,7 @@ void Ekf::calculateOutputStates() _imu_updated = false; } - if (!_output_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_output_sample_delayed)) { - return; - } + _output_sample_delayed = _output_buffer.get_oldest(); Quaternion quat_inv = _state.quat_nominal.inversed(); Quaternion q_error = _output_sample_delayed.quat_nominal * quat_inv; diff --git a/EKF/estimator_base.cpp b/EKF/estimator_base.cpp index cc48e48afc..5d3dede352 100644 --- a/EKF/estimator_base.cpp +++ b/EKF/estimator_base.cpp @@ -83,14 +83,15 @@ void EstimatorBase::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64 imu_sample_new.time_us = time_usec; - _imu_sample_new = imu_sample_new; - imu_sample_new.delta_ang(0) = imu_sample_new.delta_ang(0) * _state.gyro_scale(0); imu_sample_new.delta_ang(1) = imu_sample_new.delta_ang(1) * _state.gyro_scale(1); imu_sample_new.delta_ang(2) = imu_sample_new.delta_ang(2) * _state.gyro_scale(2); - imu_sample_new.delta_ang -= _state.gyro_bias; - imu_sample_new.delta_vel(2) -= _state.accel_z_bias; + imu_sample_new.delta_ang -= _state.gyro_bias * imu_sample_new.delta_ang_dt / (_dt_imu_avg > 0 ? _dt_imu_avg : 0.01f); + imu_sample_new.delta_vel(2) -= _state.accel_z_bias * imu_sample_new.delta_vel_dt / (_dt_imu_avg > 0 ? _dt_imu_avg : 0.01f);; + + // store the new sample for the complementary filter prediciton + _imu_sample_new = imu_sample_new; _imu_down_sampled.delta_ang_dt += imu_sample_new.delta_ang_dt; _imu_down_sampled.delta_vel_dt += imu_sample_new.delta_vel_dt;