EKF: Reduce EKF prediction delta time jitter

Make the target EKF rate an integer multiple of the IMU rate. This slightly increases the average prediction time step for the EKF from just over 10msec to 12msec, but the variation reduces significantly which makes filter tuning more deterministic.
Improve the algorithm used to adjust the collection time criteria to reduce jitter in the correction.

EKF/ekf.cpp

@@ -84,7 +84,7 @@ Ekf::Ekf():
 	_time_last_arsp_fuse(0),
 	_time_last_beta_fuse(0),
 	_last_disarmed_posD(0.0f),
-	_last_dt_overrun(0.0f),
+	_imu_collection_time_adj(0.0f),
 	_time_acc_bias_check(0.0f),
 	_airspeed_innov(0.0f),
 	_airspeed_innov_var(0.0f),
@@ -507,10 +507,12 @@ bool Ekf::collect_imu(imuSample &imu)
 	// if the target time delta between filter prediction steps has been exceeded
 	// write the accumulated IMU data to the ring buffer
 	float target_dt = (float)(FILTER_UPDATE_PERIOD_MS) / 1000;
-	if (_imu_down_sampled.delta_ang_dt >= target_dt - _last_dt_overrun) {
+	if (_imu_down_sampled.delta_ang_dt >= target_dt - _imu_collection_time_adj) {
 
-		// store the amount we have over-run the target update rate by
-		_last_dt_overrun = _imu_down_sampled.delta_ang_dt - target_dt;
+		// accumulate the amount of time to advance the IMU collection time so that we meet the
+		// average EKF update rate requirement
+		_imu_collection_time_adj += 0.01f * (_imu_down_sampled.delta_ang_dt - target_dt);
+		_imu_collection_time_adj = math::constrain(_imu_collection_time_adj, -0.5f * target_dt, 0.5f * target_dt);
 
 		imu.delta_ang     = _q_down_sampled.to_axis_angle();
 		imu.delta_vel     = _imu_down_sampled.delta_vel;

EKF/ekf.h

@@ -250,7 +250,7 @@ private:
 	uint64_t _time_last_beta_fuse;	// time the last fusion of synthetic sideslip 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_PERIOD_MS (sec)
+	float _imu_collection_time_adj;	// the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PERIOD_MS (sec)
 
 	uint64_t _time_acc_bias_check;	// last time the  accel bias check passed (usec)
 

EKF/estimator_interface.h

@@ -297,7 +297,7 @@ protected:
 	This can be adjusted to a value that is FILTER_UPDATE_PERIOD_MS longer than the maximum observation time delay.
 	*/
 	uint8_t _imu_buffer_length;
-	static const unsigned FILTER_UPDATE_PERIOD_MS = 10;	// ekf prediction period in milliseconds
+	static const unsigned FILTER_UPDATE_PERIOD_MS = 12;	// ekf prediction period in milliseconds - this should ideally be an integer multiple of the IMU time delta
 
 	unsigned _min_obs_interval_us; // minimum time interval between observations that will guarantee data is not lost (usec)