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

@@ -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;

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