ekf2: covariance update use IMU sample dt (#22032)

- usually the delta angle and delta velocity dt is the same, but they can be slightly different

src/modules/ekf2/EKF/covariance.cpp

@@ -98,15 +98,14 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 {
 	// Use average update interval to reduce accumulated covariance prediction errors due to small single frame dt values
 	const float dt = _dt_ekf_avg;
-	const float dt_inv = 1.f / dt;
 
 	// inhibit learning of imu accel bias if the manoeuvre levels are too high to protect against the effect of sensor nonlinearities or bad accel data is detected
 	// xy accel bias learning is also disabled on ground as those states are poorly observable when perpendicular to the gravity vector
 	const float alpha = math::constrain((dt / _params.acc_bias_learn_tc), 0.0f, 1.0f);
 	const float beta = 1.0f - alpha;
-	_ang_rate_magnitude_filt = fmaxf(dt_inv * imu_delayed.delta_ang.norm(), beta * _ang_rate_magnitude_filt);
-	_accel_magnitude_filt = fmaxf(dt_inv * imu_delayed.delta_vel.norm(), beta * _accel_magnitude_filt);
-	_accel_vec_filt = alpha * dt_inv * imu_delayed.delta_vel + beta * _accel_vec_filt;
+	_ang_rate_magnitude_filt = fmaxf(imu_delayed.delta_ang.norm() / imu_delayed.delta_ang_dt, beta * _ang_rate_magnitude_filt);
+	_accel_magnitude_filt = fmaxf(imu_delayed.delta_vel.norm() / imu_delayed.delta_vel_dt, beta * _accel_magnitude_filt);
+	_accel_vec_filt = alpha * imu_delayed.delta_vel / imu_delayed.delta_vel_dt + beta * _accel_vec_filt;
 
 	const bool is_manoeuvre_level_high = _ang_rate_magnitude_filt > _params.acc_bias_learn_gyr_lim
 					     || _accel_magnitude_filt > _params.acc_bias_learn_acc_lim;
@@ -216,7 +215,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 	// assign IMU noise variances
 	// inputs to the system are 3 delta angles and 3 delta velocities
 	float gyro_noise = math::constrain(_params.gyro_noise, 0.0f, 1.0f);
-	const float d_ang_var = sq(dt * gyro_noise);
+	const float d_ang_var = sq(imu_delayed.delta_ang_dt * gyro_noise);
 
 	float accel_noise = math::constrain(_params.accel_noise, 0.0f, 1.0f);
 
@@ -225,10 +224,10 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 	for (int i = 0; i <= 2; i++) {
 		if (_fault_status.flags.bad_acc_vertical || imu_delayed.delta_vel_clipping[i]) {
 			// Increase accelerometer process noise if bad accel data is detected
-			d_vel_var(i) = sq(dt * BADACC_BIAS_PNOISE);
+			d_vel_var(i) = sq(imu_delayed.delta_vel_dt * BADACC_BIAS_PNOISE);
 
 		} else {
-			d_vel_var(i) = sq(dt * accel_noise);
+			d_vel_var(i) = sq(imu_delayed.delta_vel_dt * accel_noise);
 		}
 	}
 
@@ -236,7 +235,10 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 	SquareMatrix24f nextP;
 
 	// calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states
-	sym::PredictCovariance(getStateAtFusionHorizonAsVector(), P, imu_delayed.delta_vel, d_vel_var, imu_delayed.delta_ang, d_ang_var, dt, &nextP);
+	sym::PredictCovariance(getStateAtFusionHorizonAsVector(), P,
+		imu_delayed.delta_vel, imu_delayed.delta_vel_dt, d_vel_var,
+		imu_delayed.delta_ang, imu_delayed.delta_ang_dt, d_ang_var,
+		&nextP);
 
 	// compute noise variance for stationary processes
 	Vector24f process_noise;

src/modules/ekf2/EKF/python/ekf_derivation/derivation.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 # -*- coding: utf-8 -*-
 """
     Copyright (c) 2022 PX4 Development Team
@@ -82,19 +82,20 @@ def predict_covariance(
         state: VState,
         P: MState,
         d_vel: sf.V3,
+        d_vel_dt: sf.Scalar,
         d_vel_var: sf.V3,
         d_ang: sf.V3,
-        d_ang_var: sf.Scalar,
-        dt: sf.Scalar
+        d_ang_dt: sf.Scalar,
+        d_ang_var: sf.Scalar
 ):
     g = sf.Symbol("g") # does not appear in the jacobians
 
     accel_b = sf.V3(state[State.accel_bx], state[State.accel_by], state[State.accel_bz])
-    d_vel_b = accel_b * dt
+    d_vel_b = accel_b * d_vel_dt
     d_vel_true = d_vel - d_vel_b
 
     gyro_b = sf.V3(state[State.gyro_bx], state[State.gyro_by], state[State.gyro_bz])
-    d_ang_b = gyro_b * dt
+    d_ang_b = gyro_b * d_ang_dt
     d_ang_true = d_ang - d_ang_b
 
     q = state_to_quat(state)
@@ -103,8 +104,8 @@ def predict_covariance(
     p = sf.V3(state[State.px], state[State.py], state[State.pz])
 
     q_new = q * sf.Quaternion(sf.V3(0.5 * d_ang_true[0], 0.5 * d_ang_true[1], 0.5 * d_ang_true[2]), 1)
-    v_new = v + R_to_earth * d_vel_true + sf.V3(0, 0, g) * dt
-    p_new = p + v * dt
+    v_new = v + R_to_earth * d_vel_true + sf.V3(0, 0, g) * d_vel_dt
+    p_new = p + v * d_vel_dt
 
     # Predicted state vector at time t + dt
     state_new = VState.block_matrix([[sf.V4(q_new.w, q_new.x, q_new.y, q_new.z)], [v_new], [p_new], [sf.Matrix(state[State.gyro_bx:State.n_states])]])