ekf2: add IMU delta_ang_dt/delta_vel_dt safety constrain before pushi…

…ng into buffer

src/modules/ekf2/EKF/covariance.cpp

@@ -131,8 +131,8 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 
 	// calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states
 	P = sym::PredictCovariance(_state.vector(), P,
-				   imu_delayed.delta_vel / math::max(imu_delayed.delta_vel_dt, FLT_EPSILON), accel_var,
-				   imu_delayed.delta_ang / math::max(imu_delayed.delta_ang_dt, FLT_EPSILON), gyro_var,
+				   imu_delayed.delta_vel / imu_delayed.delta_vel_dt, accel_var,
+				   imu_delayed.delta_ang / imu_delayed.delta_ang_dt, gyro_var,
 				   dt);
 
 	// Construct the process noise variance diagonal for those states with a stationary process model

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -93,7 +93,17 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
 
 		_imu_updated = true;
 
-		_imu_buffer.push(_imu_down_sampler.getDownSampledImuAndTriggerReset());
+		imuSample imu_downsampled = _imu_down_sampler.getDownSampledImuAndTriggerReset();
+
+		// as a precaution constrain the integration delta time to prevent numerical problems
+		const float filter_update_period_s = _params.filter_update_interval_us * 1e-6f;
+		const float imu_min_dt = 0.5f * filter_update_period_s;
+		const float imu_max_dt = 2.0f * filter_update_period_s;
+
+		imu_downsampled.delta_ang_dt = math::constrain(imu_downsampled.delta_ang_dt, imu_min_dt, imu_max_dt);
+		imu_downsampled.delta_vel_dt = math::constrain(imu_downsampled.delta_vel_dt, imu_min_dt, imu_max_dt);
+
+		_imu_buffer.push(imu_downsampled);
 
 		// get the oldest data from the buffer
 		_time_delayed_us = _imu_buffer.get_oldest().time_us;