From f1b7e7714e39626d2ddc1aed5d25d925558ef71f Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 5 May 2016 19:49:28 +1000 Subject: [PATCH] EKF: Make average update rate of EKF closer to target With the EKF, the average update rate is more important than the instantaneous value as it affects tuning. This patch ensures that the EKF prediction cycle will be performed early if the previous one was late in an attempt to maintain the target update rate. --- EKF/ekf.cpp | 9 +++++++-- EKF/ekf.h | 1 + 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index bf6a52ef0f..0e02296d73 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -73,6 +73,7 @@ Ekf::Ekf(): _time_last_of_fuse(0), _time_last_arsp_fuse(0), _last_disarmed_posD(0.0f), + _last_dt_overrun(0.0f), _airspeed_innov(0.0f), _airspeed_innov_var(0.0f), _heading_innov(0.0f), @@ -584,8 +585,12 @@ bool Ekf::collect_imu(imuSample &imu) _imu_down_sampled.delta_vel += (_imu_sample_new.delta_vel + delta_R * _imu_sample_new.delta_vel) * 0.5f; // if the target time delta between filter prediction steps has been exceeded - // write the accumulated IMu data to the ring buffer - if (_imu_down_sampled.delta_ang_dt >= (float)(FILTER_UPDATE_PERRIOD_MS) / 1000) { + // write the accumulated IMU data to the ring buffer + float target_dt = (float)(FILTER_UPDATE_PERRIOD_MS) / 1000; + if (_imu_down_sampled.delta_ang_dt >= target_dt - _last_dt_overrun) { + + // store the amount we have over-run the target update rate by + _last_dt_overrun = _imu_down_sampled.delta_ang_dt - target_dt; imu.delta_ang = _q_down_sampled.to_axis_angle(); imu.delta_vel = _imu_down_sampled.delta_vel; diff --git a/EKF/ekf.h b/EKF/ekf.h index f496f64046..8cb99797cb 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -159,6 +159,7 @@ private: uint64_t _time_last_arsp_fuse; // time the last fusion of airspeed measurements were performed (usec) Vector2f _last_known_posNE; // last known local NE position vector (m) float _last_disarmed_posD; // vertical position recorded at arming (m) + float _last_dt_overrun; // the amount of time the last IMU collection over-ran the target set by FILTER_UPDATE_PERRIOD_MS (sec) Vector3f _earth_rate_NED; // earth rotation vector (NED) in rad/s