diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 8108d26c1381..58e83b3bd757 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -134,8 +134,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 diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index bce6ec88637f..d4a579f5b8cf 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -94,7 +94,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;