ekf2: change delta angle and delta velocity bias states to accel and gyro bias (#21901)

* ekf2-test: remove outdated codegen comparison

The definition of states changed so the comparison with the old
derivation cannot work anymore.

---------

Co-authored-by: bresch <brescianimathieu@gmail.com>

src/modules/ekf2/EKF/EKFGSF_yaw.h

@@ -39,7 +39,6 @@
 #include <mathlib/mathlib.h>
 
 #include "common.h"
-#include "utils.hpp"
 
 using matrix::AxisAnglef;
 using matrix::Dcmf;

src/modules/ekf2/EKF/common.h

@@ -276,8 +276,8 @@ struct stateSample {
 	Quatf    quat_nominal{};        ///< quaternion defining the rotation from body to earth frame
 	Vector3f vel{};                 ///< NED velocity in earth frame in m/s
 	Vector3f pos{};                 ///< NED position in earth frame in m
-	Vector3f delta_ang_bias{};      ///< delta angle bias estimate in rad
-	Vector3f delta_vel_bias{};      ///< delta velocity bias estimate in m/s
+	Vector3f gyro_bias{};           ///< gyro bias estimate in rad/s
+	Vector3f accel_bias{};          ///< accel bias estimate in m/s^2
 	Vector3f mag_I{};               ///< NED earth magnetic field in gauss
 	Vector3f mag_B{};               ///< magnetometer bias estimate in body frame in gauss
 	Vector2f wind_vel{};            ///< horizontal wind velocity in earth frame in m/s

src/modules/ekf2/EKF/covariance.cpp

@@ -43,7 +43,6 @@
 
 #include "ekf.h"
 #include "python/ekf_derivation/generated/predict_covariance.h"
-#include "utils.hpp"
 
 #include <math.h>
 #include <mathlib/mathlib.h>
@@ -54,11 +53,6 @@ void Ekf::initialiseCovariance()
 {
 	P.zero();
 
-	_delta_angle_bias_var_accum.setZero();
-	_delta_vel_bias_var_accum.setZero();
-
-	const float dt = _dt_ekf_avg;
-
 	resetQuatCov();
 
 	// velocity
@@ -82,14 +76,14 @@ void Ekf::initialiseCovariance()
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 	// gyro bias
-	_prev_delta_ang_bias_var(0) = P(10,10) = sq(_params.switch_on_gyro_bias * dt);
-	_prev_delta_ang_bias_var(1) = P(11,11) = P(10,10);
-	_prev_delta_ang_bias_var(2) = P(12,12) = P(10,10);
+	_prev_gyro_bias_var(0) = P(10,10) = sq(_params.switch_on_gyro_bias);
+	_prev_gyro_bias_var(1) = P(11,11) = P(10,10);
+	_prev_gyro_bias_var(2) = P(12,12) = P(10,10);
 
 	// accel bias
-	_prev_dvel_bias_var(0) = P(13,13) = sq(_params.switch_on_accel_bias * dt);
-	_prev_dvel_bias_var(1) = P(14,14) = P(13,13);
-	_prev_dvel_bias_var(2) = P(15,15) = P(13,13);
+	_prev_accel_bias_var(0) = P(13,13) = sq(_params.switch_on_accel_bias);
+	_prev_accel_bias_var(1) = P(14,14) = P(13,13);
+	_prev_accel_bias_var(2) = P(15,15) = P(13,13);
 
 	resetMagCov();
 
@@ -105,12 +99,6 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 	const float dt = _dt_ekf_avg;
 	const float dt_inv = 1.f / dt;
 
-	// convert rate of change of rate gyro bias (rad/s**2) as specified by the parameter to an expected change in delta angle (rad) since the last update
-	const float d_ang_bias_sig = dt * dt * math::constrain(_params.gyro_bias_p_noise, 0.0f, 1.0f);
-
-	// convert rate of change of accelerometer bias (m/s**3) as specified by the parameter to an expected change in delta velocity (m/s) since the last update
-	const float d_vel_bias_sig = dt * dt * math::constrain(_params.accel_bias_p_noise, 0.0f, 1.0f);
-
 	// 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);
@@ -137,14 +125,14 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 		if (do_inhibit_axis) {
 			// store the bias state variances to be reinstated later
 			if (!_gyro_bias_inhibit[index]) {
-				_prev_delta_ang_bias_var(index) = P(stateIndex, stateIndex);
+				_prev_gyro_bias_var(index) = P(stateIndex, stateIndex);
 				_gyro_bias_inhibit[index] = true;
 			}
 
 		} else {
 			if (_gyro_bias_inhibit[index]) {
 				// reinstate the bias state variances
-				P(stateIndex, stateIndex) = _prev_delta_ang_bias_var(index);
+				P(stateIndex, stateIndex) = _prev_gyro_bias_var(index);
 				_gyro_bias_inhibit[index] = false;
 			}
 		}
@@ -176,14 +164,14 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 		if (do_inhibit_axis) {
 			// store the bias state variances to be reinstated later
 			if (!_accel_bias_inhibit[index]) {
-				_prev_dvel_bias_var(index) = P(stateIndex, stateIndex);
+				_prev_accel_bias_var(index) = P(stateIndex, stateIndex);
 				_accel_bias_inhibit[index] = true;
 			}
 
 		} else {
 			if (_accel_bias_inhibit[index]) {
 				// reinstate the bias state variances
-				P(stateIndex, stateIndex) = _prev_dvel_bias_var(index);
+				P(stateIndex, stateIndex) = _prev_accel_bias_var(index);
 				_accel_bias_inhibit[index] = false;
 			}
 		}
@@ -223,22 +211,6 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 		wind_vel_nsd_scaled = 0.0f;
 	}
 
-	// compute noise variance for stationary processes
-	Vector24f process_noise;
-
-	// Construct the process noise variance diagonal for those states with a stationary process model
-	// These are kinematic states and their error growth is controlled separately by the IMU noise variances
-
-	// delta angle bias states
-	process_noise.slice<3, 1>(10, 0) = sq(d_ang_bias_sig);
-	// delta_velocity bias states
-	process_noise.slice<3, 1>(13, 0) = sq(d_vel_bias_sig);
-	// earth frame magnetic field states
-	process_noise.slice<3, 1>(16, 0) = sq(mag_I_sig);
-	// body frame magnetic field states
-	process_noise.slice<3, 1>(19, 0) = sq(mag_B_sig);
-	// wind velocity states
-	process_noise.slice<2, 1>(22, 0) = sq(wind_vel_nsd_scaled) * dt;
 
 	// assign IMU noise variances
 	// inputs to the system are 3 delta angles and 3 delta velocities
@@ -265,43 +237,52 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 	// 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);
 
-	// process noise contribution for delta angle states can be very small compared to
-	// the variances, therefore use algorithm to minimise numerical error
-	for (unsigned i = 10; i <= 12; i++) {
-		const int index = i - 10;
+	// compute noise variance for stationary processes
+	Vector24f process_noise;
 
-		if (!_gyro_bias_inhibit[index]) {
-			// add process noise that is not from the IMU
-			// process noise contribution for delta velocity states can be very small compared to
-			// the variances, therefore use algorithm to minimise numerical error
-			nextP(i, i) = kahanSummation(nextP(i, i), process_noise(i), _delta_angle_bias_var_accum(index));
+	// Construct the process noise variance diagonal for those states with a stationary process model
+	// These are kinematic states and their error growth is controlled separately by the IMU noise variances
 
-		} else {
-			nextP.uncorrelateCovarianceSetVariance<1>(i, _prev_delta_ang_bias_var(index));
-			_delta_angle_bias_var_accum(index) = 0.f;
-		}
-	}
-
-	for (int i = 13; i <= 15; i++) {
-		const int index = i - 13;
-
-		if (!_accel_bias_inhibit[index]) {
-			// add process noise that is not from the IMU
-			// process noise contribution for delta velocity states can be very small compared to
-			// the variances, therefore use algorithm to minimise numerical error
-			nextP(i, i) = kahanSummation(nextP(i, i), process_noise(i), _delta_vel_bias_var_accum(index));
-
-		} else {
-			nextP.uncorrelateCovarianceSetVariance<1>(i, _prev_dvel_bias_var(index));
-			_delta_vel_bias_var_accum(index) = 0.f;
-		}
-	}
+	// earth frame magnetic field states
+	process_noise.slice<3, 1>(16, 0) = sq(mag_I_sig);
+	// body frame magnetic field states
+	process_noise.slice<3, 1>(19, 0) = sq(mag_B_sig);
+	// wind velocity states
+	process_noise.slice<2, 1>(22, 0) = sq(wind_vel_nsd_scaled) * dt;
 
 	// add process noise that is not from the IMU
 	for (unsigned i = 16; i <= 23; i++) {
 		nextP(i, i) += process_noise(i);
 	}
 
+	// gyro bias: add process noise, or restore previous gyro bias var if state inhibited
+	const float gyro_bias_sig = dt * math::constrain(_params.gyro_bias_p_noise, 0.f, 1.f);
+	const float gyro_bias_process_noise = sq(gyro_bias_sig);
+	for (unsigned i = 10; i <= 12; i++) {
+		const int axis_index = i - 10;
+
+		if (!_gyro_bias_inhibit[axis_index]) {
+			nextP(i, i) += gyro_bias_process_noise;
+
+		} else {
+			nextP.uncorrelateCovarianceSetVariance<1>(i, _prev_gyro_bias_var(axis_index));
+		}
+	}
+
+	// accel bias: add process noise, or restore previous accel bias var if state inhibited
+	const float accel_bias_sig = dt * math::constrain(_params.accel_bias_p_noise, 0.f, 1.f);
+	const float accel_bias_process_noise = sq(accel_bias_sig);
+	for (int i = 13; i <= 15; i++) {
+		const int axis_index = i - 13;
+
+		if (!_accel_bias_inhibit[axis_index]) {
+			nextP(i, i) += accel_bias_process_noise;
+
+		} else {
+			nextP.uncorrelateCovarianceSetVariance<1>(i, _prev_accel_bias_var(axis_index));
+		}
+	}
+
 	// covariance matrix is symmetrical, so copy upper half to lower half
 	for (unsigned row = 0; row <= 15; row++) {
 		for (unsigned column = 0 ; column < row; column++) {
@@ -385,7 +366,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 	// accelerometer bias states
 	if (!_accel_bias_inhibit[0] || !_accel_bias_inhibit[1] || !_accel_bias_inhibit[2]) {
 		// Find the maximum delta velocity bias state variance and request a covariance reset if any variance is below the safe minimum
-		const float minSafeStateVar = 1e-9f;
+		const float minSafeStateVar = 1e-9f / sq(_dt_ekf_avg);
 		float maxStateVar = minSafeStateVar;
 		bool resetRequired = false;
 
@@ -406,7 +387,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 		// To ensure stability of the covariance matrix operations, the ratio of a max and min variance must
 		// not exceed 100 and the minimum variance must not fall below the target minimum
 		// Also limit variance to a maximum equivalent to a 0.1g uncertainty
-		const float minStateVarTarget = 5E-8f;
+		const float minStateVarTarget = 5E-8f / sq(_dt_ekf_avg);
 		float minAllowedStateVar = fmaxf(0.01f * maxStateVar, minStateVarTarget);
 
 		for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) {
@@ -415,8 +396,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 				continue;
 			}
 
-			P(stateIndex, stateIndex) = math::constrain(P(stateIndex, stateIndex), minAllowedStateVar,
-						    sq(0.1f * CONSTANTS_ONE_G * _dt_ekf_avg));
+			P(stateIndex, stateIndex) = math::constrain(P(stateIndex, stateIndex), minAllowedStateVar, sq(0.1f * CONSTANTS_ONE_G));
 		}
 
 		// If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero
@@ -427,7 +407,8 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 		// Run additional checks to see if the delta velocity bias has hit limits in a direction that is clearly wrong
 		// calculate accel bias term aligned with the gravity vector
 		const float dVel_bias_lim = 0.9f * _params.acc_bias_lim * _dt_ekf_avg;
-		const float down_dvel_bias = _state.delta_vel_bias.dot(Vector3f(_R_to_earth.row(2)));
+		const Vector3f delta_vel_bias = _state.accel_bias * _dt_ekf_avg;
+		const float down_dvel_bias = delta_vel_bias.dot(Vector3f(_R_to_earth.row(2)));
 
 		// check that the vertical component of accel bias is consistent with both the vertical position and velocity innovation
 		bool bad_acc_bias = false;
@@ -602,9 +583,9 @@ void Ekf::zeroMagCov()
 	P.uncorrelateCovarianceSetVariance<3>(19, 0.0f);
 }
 
-void Ekf::resetZDeltaAngBiasCov()
+void Ekf::resetGyroBiasZCov()
 {
-	const float init_delta_ang_bias_var = sq(_params.switch_on_gyro_bias * _dt_ekf_avg);
+	const float init_gyro_bias_var = sq(_params.switch_on_gyro_bias);
 
-	P.uncorrelateCovarianceSetVariance<1>(12, init_delta_ang_bias_var);
+	P.uncorrelateCovarianceSetVariance<1>(12, init_gyro_bias_var);
 }

src/modules/ekf2/EKF/drag_fusion.cpp

@@ -110,7 +110,7 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
 	// perform sequential fusion of XY specific forces
 	for (uint8_t axis_index = 0; axis_index < 2; axis_index++) {
 		// measured drag acceleration corrected for sensor bias
-		const float mea_acc = drag_sample.accelXY(axis_index)  - _state.delta_vel_bias(axis_index) / _dt_ekf_avg;
+		const float mea_acc = drag_sample.accelXY(axis_index) - _state.accel_bias(axis_index);
 
 		// Drag is modelled as an arbitrary combination of bluff body drag that proportional to
 		// equivalent airspeed squared, and rotor momentum drag that is proportional to true airspeed

src/modules/ekf2/EKF/ekf.cpp

@@ -56,8 +56,8 @@ void Ekf::reset()
 
 	_state.vel.setZero();
 	_state.pos.setZero();
-	_state.delta_ang_bias.setZero();
-	_state.delta_vel_bias.setZero();
+	_state.gyro_bias.setZero();
+	_state.accel_bias.setZero();
 	_state.mag_I.setZero();
 	_state.mag_B.setZero();
 	_state.wind_vel.setZero();
@@ -80,7 +80,8 @@ void Ekf::reset()
 	_fault_status.value = 0;
 	_innov_check_fail_status.value = 0;
 
-	_prev_dvel_bias_var.zero();
+	_prev_gyro_bias_var.zero();
+	_prev_accel_bias_var.zero();
 
 	resetGpsDriftCheckFilters();
 
@@ -185,8 +186,7 @@ bool Ekf::update()
 		runTerrainEstimator(imu_sample_delayed);
 #endif // CONFIG_EKF2_RANGE_FINDER
 
-		_output_predictor.correctOutputStates(imu_sample_delayed.time_us, getGyroBias(), getAccelBias(),
-							_state.quat_nominal, _state.vel, _state.pos);
+		_output_predictor.correctOutputStates(imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _state.pos, _state.gyro_bias, _state.accel_bias);
 
 		return true;
 	}

src/modules/ekf2/EKF/ekf.h

@@ -363,13 +363,13 @@ public:
 	}
 
 	// gyro bias (states 10, 11, 12)
-	Vector3f getGyroBias() const { return _state.delta_ang_bias / _dt_ekf_avg; } // get the gyroscope bias in rad/s
-	Vector3f getGyroBiasVariance() const { return Vector3f{P(10, 10), P(11, 11), P(12, 12)} / sq(_dt_ekf_avg); } // get the gyroscope bias variance in rad/s
+	const Vector3f &getGyroBias() const { return _state.gyro_bias; } // get the gyroscope bias in rad/s
+	Vector3f getGyroBiasVariance() const { return Vector3f{P(10, 10), P(11, 11), P(12, 12)}; } // get the gyroscope bias variance in rad/s
 	float getGyroBiasLimit() const { return _params.gyro_bias_lim; }
 
 	// accel bias (states 13, 14, 15)
-	Vector3f getAccelBias() const { return _state.delta_vel_bias / _dt_ekf_avg; } // get the accelerometer bias in m/s**2
-	Vector3f getAccelBiasVariance() const { return Vector3f{P(13, 13), P(14, 14), P(15, 15)} / sq(_dt_ekf_avg); } // get the accelerometer bias variance in m/s**2
+	const Vector3f &getAccelBias() const { return _state.accel_bias; } // get the accelerometer bias in m/s**2
+	Vector3f getAccelBiasVariance() const { return Vector3f{P(13, 13), P(14, 14), P(15, 15)}; } // get the accelerometer bias variance in m/s**2
 	float getAccelBiasLimit() const { return _params.acc_bias_lim; }
 
 	const Vector3f &getMagEarthField() const { return _state.mag_I; }
@@ -589,9 +589,6 @@ private:
 
 	SquareMatrix24f P{};	///< state covariance matrix
 
-	Vector3f _delta_angle_bias_var_accum{};	///< kahan summation algorithm accumulator for delta angle bias variance
-	Vector3f _delta_vel_bias_var_accum{};   ///< kahan summation algorithm accumulator for delta velocity bias variance
-
 #if defined(CONFIG_EKF2_DRAG_FUSION)
 	Vector2f _drag_innov{};		///< multirotor drag measurement innovation (m/sec**2)
 	Vector2f _drag_innov_var{};	///< multirotor drag measurement innovation variance ((m/sec**2)**2)
@@ -721,8 +718,9 @@ private:
 	Vector3f _accel_vec_filt{};		///< acceleration vector after application of a low pass filter (m/sec**2)
 	float _accel_magnitude_filt{0.0f};	///< acceleration magnitude after application of a decaying envelope filter (rad/sec)
 	float _ang_rate_magnitude_filt{0.0f};		///< angular rate magnitude after application of a decaying envelope filter (rad/sec)
-	Vector3f _prev_delta_ang_bias_var{};	///< saved delta angle XYZ bias variances (rad/sec)
-	Vector3f _prev_dvel_bias_var{};		///< saved delta velocity XYZ bias variances (m/sec)**2
+
+	Vector3f _prev_gyro_bias_var{};         ///< saved gyro XYZ bias variances
+	Vector3f _prev_accel_bias_var{};        ///< saved accel XYZ bias variances
 
 	// height sensor status
 	bool _baro_hgt_faulty{false};		///< true if baro data have been declared faulty TODO: move to fault flags
@@ -1110,7 +1108,7 @@ private:
 	void clearMagCov();
 	void zeroMagCov();
 
-	void resetZDeltaAngBiasCov();
+	void resetGyroBiasZCov();
 
 	// uncorrelate quaternion states from other states
 	void uncorrelateQuatFromOtherStates();

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -224,11 +224,11 @@ void Ekf::constrainStates()
 	_state.vel = matrix::constrain(_state.vel, -1000.0f, 1000.0f);
 	_state.pos = matrix::constrain(_state.pos, -1.e6f, 1.e6f);
 
-	const float delta_ang_bias_limit = getGyroBiasLimit() * _dt_ekf_avg;
-	_state.delta_ang_bias = matrix::constrain(_state.delta_ang_bias, -delta_ang_bias_limit, delta_ang_bias_limit);
+	const float gyro_bias_limit = getGyroBiasLimit();
+	_state.gyro_bias = matrix::constrain(_state.gyro_bias, -gyro_bias_limit, gyro_bias_limit);
 
-	const float delta_vel_bias_limit = getAccelBiasLimit() * _dt_ekf_avg;
-	_state.delta_vel_bias = matrix::constrain(_state.delta_vel_bias, -delta_vel_bias_limit, delta_vel_bias_limit);
+	const float accel_bias_limit = getAccelBiasLimit();
+	_state.accel_bias = matrix::constrain(_state.accel_bias, -accel_bias_limit, accel_bias_limit);
 
 	_state.mag_I = matrix::constrain(_state.mag_I, -1.0f, 1.0f);
 	_state.mag_B = matrix::constrain(_state.mag_B, -getMagBiasLimit(), getMagBiasLimit());
@@ -390,8 +390,8 @@ matrix::Vector<float, 24> Ekf::getStateAtFusionHorizonAsVector() const
 	state.slice<4, 1>(0, 0) = _state.quat_nominal;
 	state.slice<3, 1>(4, 0) = _state.vel;
 	state.slice<3, 1>(7, 0) = _state.pos;
-	state.slice<3, 1>(10, 0) = _state.delta_ang_bias;
-	state.slice<3, 1>(13, 0) = _state.delta_vel_bias;
+	state.slice<3, 1>(10, 0) = _state.gyro_bias;
+	state.slice<3, 1>(13, 0) = _state.accel_bias;
 	state.slice<3, 1>(16, 0) = _state.mag_I;
 	state.slice<3, 1>(19, 0) = _state.mag_B;
 	state.slice<2, 1>(22, 0) = _state.wind_vel;
@@ -612,25 +612,28 @@ void Ekf::resetImuBias()
 
 void Ekf::resetGyroBias()
 {
-	// Zero the delta angle and delta velocity bias states
-	_state.delta_ang_bias.zero();
+	// Zero the gyro bias states
+	_state.gyro_bias.zero();
 
 	// Zero the corresponding covariances and set
 	// variances to the values use for initial alignment
-	P.uncorrelateCovarianceSetVariance<3>(10, sq(_params.switch_on_gyro_bias * _dt_ekf_avg));
+	P.uncorrelateCovarianceSetVariance<3>(10, sq(_params.switch_on_gyro_bias));
+
+	// Set previous frame values
+	_prev_gyro_bias_var = P.slice<3, 3>(10, 10).diag();
 }
 
 void Ekf::resetAccelBias()
 {
-	// Zero the delta angle and delta velocity bias states
-	_state.delta_vel_bias.zero();
+	// Zero the accel bias states
+	_state.accel_bias.zero();
 
 	// Zero the corresponding covariances and set
 	// variances to the values use for initial alignment
-	P.uncorrelateCovarianceSetVariance<3>(13, sq(_params.switch_on_accel_bias * _dt_ekf_avg));
+	P.uncorrelateCovarianceSetVariance<3>(13, sq(_params.switch_on_accel_bias));
 
 	// Set previous frame values
-	_prev_dvel_bias_var = P.slice<3, 3>(13, 13).diag();
+	_prev_accel_bias_var = P.slice<3, 3>(13, 13).diag();
 }
 
 // get EKF innovation consistency check status information comprising of:
@@ -792,13 +795,13 @@ void Ekf::fuse(const Vector24f &K, float innovation)
 	_state.quat_nominal.normalize();
 	_R_to_earth = Dcmf(_state.quat_nominal);
 
-	_state.vel -= K.slice<3, 1>(4, 0) * innovation;
-	_state.pos -= K.slice<3, 1>(7, 0) * innovation;
-	_state.delta_ang_bias -= K.slice<3, 1>(10, 0) * innovation;
-	_state.delta_vel_bias -= K.slice<3, 1>(13, 0) * innovation;
-	_state.mag_I -= K.slice<3, 1>(16, 0) * innovation;
-	_state.mag_B -= K.slice<3, 1>(19, 0) * innovation;
-	_state.wind_vel -= K.slice<2, 1>(22, 0) * innovation;
+	_state.vel        -= K.slice<3, 1>(4, 0)  * innovation;
+	_state.pos        -= K.slice<3, 1>(7, 0)  * innovation;
+	_state.gyro_bias  -= K.slice<3, 1>(10, 0) * innovation;
+	_state.accel_bias -= K.slice<3, 1>(13, 0) * innovation;
+	_state.mag_I      -= K.slice<3, 1>(16, 0) * innovation;
+	_state.mag_B      -= K.slice<3, 1>(19, 0) * innovation;
+	_state.wind_vel   -= K.slice<2, 1>(22, 0) * innovation;
 }
 
 void Ekf::uncorrelateQuatFromOtherStates()

src/modules/ekf2/EKF/estimator_interface.h

@@ -64,7 +64,6 @@
 #include "common.h"
 #include "RingBuffer.h"
 #include "imu_down_sampler.hpp"
-#include "utils.hpp"
 #include "output_predictor.h"
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)

src/modules/ekf2/EKF/gps_yaw_fusion.cpp

@@ -126,7 +126,7 @@ void Ekf::fuseGpsYaw()
 		// A constant large signed test ratio is a sign of wrong gyro bias
 		// Reset the yaw gyro variance to converge faster and avoid
 		// being stuck on a previous bad estimate
-		resetZDeltaAngBiasCov();
+		resetGyroBiasZCov();
 	}
 
 	// calculate the Kalman gains

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -296,7 +296,7 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s& aid_src_status, const Vector24f &H_Y
 
 				// also reset the yaw gyro variance to converge faster and avoid
 				// being stuck on a previous bad estimate
-				resetZDeltaAngBiasCov();
+				resetGyroBiasZCov();
 
 			} else {
 				return false;

src/modules/ekf2/EKF/optflow_fusion.cpp

@@ -47,7 +47,6 @@
 #include <float.h>
 #include "python/ekf_derivation/generated/compute_flow_xy_innov_var_and_hx.h"
 #include "python/ekf_derivation/generated/compute_flow_y_innov_var_and_h.h"
-#include "utils.hpp"
 
 void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
 {

src/modules/ekf2/EKF/output_predictor.cpp

@@ -240,8 +240,7 @@ void OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector
 }
 
 void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us,
-		const matrix::Vector3f &gyro_bias, const matrix::Vector3f &accel_bias,
-		const Quatf &quat_state, const Vector3f &vel_state, const Vector3f &pos_state)
+		const Quatf &quat_state, const Vector3f &vel_state, const Vector3f &pos_state, const matrix::Vector3f &gyro_bias, const matrix::Vector3f &accel_bias)
 {
 	// calculate an average filter update time
 	if (_time_last_correct_states_us != 0) {

src/modules/ekf2/EKF/output_predictor.h

@@ -68,8 +68,7 @@ public:
 				   const matrix::Vector3f &delta_velocity, const float delta_velocity_dt);
 
 	void correctOutputStates(const uint64_t time_delayed_us,
-				 const matrix::Vector3f &gyro_bias, const matrix::Vector3f &accel_bias,
-				 const matrix::Quatf &quat_state, const matrix::Vector3f &vel_state, const matrix::Vector3f &pos_state);
+				 const matrix::Quatf &quat_state, const matrix::Vector3f &vel_state, const matrix::Vector3f &pos_state, const matrix::Vector3f &gyro_bias, const matrix::Vector3f &accel_bias);
 
 	void resetQuaternion(const matrix::Quatf &quat_change);
 

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

@@ -50,12 +50,12 @@ class State:
     px = 7
     py = 8
     pz = 9
-    d_ang_bx = 10
-    d_ang_by = 11
-    d_ang_bz = 12
-    d_vel_bx = 13
-    d_vel_by = 14
-    d_vel_bz = 15
+    gyro_bx  = 10
+    gyro_by  = 11
+    gyro_bz  = 12
+    accel_bx = 13
+    accel_by = 14
+    accel_bz = 15
     ix = 16
     iy = 17
     iz = 18
@@ -89,10 +89,12 @@ def predict_covariance(
 ):
     g = sf.Symbol("g") # does not appear in the jacobians
 
-    d_vel_b = sf.V3(state[State.d_vel_bx], state[State.d_vel_by], state[State.d_vel_bz])
+    accel_b = sf.V3(state[State.accel_bx], state[State.accel_by], state[State.accel_bz])
+    d_vel_b = accel_b * dt
     d_vel_true = d_vel - d_vel_b
 
-    d_ang_b = sf.V3(state[State.d_ang_bx], state[State.d_ang_by], state[State.d_ang_bz])
+    gyro_b = sf.V3(state[State.gyro_bx], state[State.gyro_by], state[State.gyro_bz])
+    d_ang_b = gyro_b * dt
     d_ang_true = d_ang - d_ang_b
 
     q = state_to_quat(state)
@@ -100,12 +102,12 @@ def predict_covariance(
     v = sf.V3(state[State.vx], state[State.vy], state[State.vz])
     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
+    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
 
     # 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.d_ang_bx:State.n_states])]])
+    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])]])
 
     # State propagation jacobian
     A = state_new.jacobian(state)

src/modules/ekf2/EKF/utils.hpp

@@ -1,75 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include <matrix/math.hpp>
-
-#ifndef EKF_UTILS_HPP
-#define EKF_UTILS_HPP
-
-// Use Kahan summation algorithm to get the sum of "sum_previous" and "input".
-// This function relies on the caller to be responsible for keeping a copy of
-// "accumulator" and passing this value at the next iteration.
-// Ref: https://en.wikipedia.org/wiki/Kahan_summation_algorithm
-inline float kahanSummation(float sum_previous, float input, float &accumulator)
-{
-	const float y = input - accumulator;
-	const float t = sum_previous + y;
-	accumulator = (t - sum_previous) - y;
-	return t;
-}
-
-namespace ecl
-{
-inline float powf(float x, int exp)
-{
-	float ret;
-
-	if (exp > 0) {
-		ret = x;
-
-		for (int count = 1; count < exp; count++) {
-			ret *= x;
-		}
-
-		return ret;
-
-	} else if (exp < 0) {
-		return 1.0f / ecl::powf(x, -exp);
-	}
-
-	return 1.0f;
-}
-
-} // namespace ecl
-
-#endif // EKF_UTILS_HPP

src/modules/ekf2/EKF/zero_gyro_update.cpp

@@ -54,12 +54,10 @@ void Ekf::controlZeroGyroUpdate(const imuSample &imu_delayed)
 		const bool zero_gyro_update_data_ready = _zgup_delta_ang_dt >= zgup_dt;
 
 		if (zero_gyro_update_data_ready) {
-			Vector3f delta_ang_scaled = _zgup_delta_ang / _zgup_delta_ang_dt * _dt_ekf_avg;
-			Vector3f innovation = _state.delta_ang_bias - delta_ang_scaled;
+			Vector3f gyro_bias = _zgup_delta_ang / _zgup_delta_ang_dt;
+			Vector3f innovation = _state.gyro_bias - gyro_bias;
 
-			const float d_ang_sig = _dt_ekf_avg * math::constrain(_params.gyro_noise, 0.0f, 1.0f);
-			//const float obs_var = sq(d_ang_sig) * (_dt_ekf_avg / _zgup_delta_ang_dt); // This is correct but too small for single precision
-			const float obs_var = sq(d_ang_sig);
+			const float obs_var = sq(math::constrain(_params.gyro_noise, 0.f, 1.f));
 
 			Vector3f innov_var{
 				P(10, 10) + obs_var,

src/modules/ekf2/test/CMakeLists.txt

@@ -40,7 +40,6 @@ px4_add_unit_gtest(SRC test_EKF_accelerometer.cpp LINKLIBS ecl_EKF ecl_sensor_si
 px4_add_unit_gtest(SRC test_EKF_airspeed_fusion_generated.cpp LINKLIBS ecl_EKF ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_airspeed.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_basics.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
-px4_add_unit_gtest(SRC test_EKF_covariance_prediction_generated.cpp LINKLIBS ecl_EKF ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_externalVision.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_flow.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_gyroscope.cpp LINKLIBS ecl_EKF ecl_sensor_sim)

src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp

@@ -288,11 +288,6 @@ int EkfWrapper::getQuaternionResetCounter() const
 	return static_cast<int>(counter);
 }
 
-matrix::Vector3f EkfWrapper::getDeltaVelBiasVariance() const
-{
-	return _ekf->covariances_diagonal().slice<3, 1>(13, 0);
-}
-
 void EkfWrapper::enableDragFusion()
 {
 	_ekf_params->drag_ctrl = 1;

src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h

@@ -120,8 +120,6 @@ public:
 	matrix::Vector4f getQuaternionVariance() const;
 	int getQuaternionResetCounter() const;
 
-	matrix::Vector3f getDeltaVelBiasVariance() const;
-
 	void enableDragFusion();
 	void disableDragFusion();
 	void setDragFusionParameters(const float &bcoef_x, const float &bcoef_y, const float &mcoef);

src/modules/ekf2/test/test_EKF_airspeed_fusion_generated.cpp

@@ -62,7 +62,7 @@ TEST(AirspeedFusionGenerated, SympyVsSymforce)
 		// Intermediate variables
 		const float HK0 = vn - vwn;
 		const float HK1 = ve - vwe;
-		const float HK2 = ecl::powf(HK0, 2) + ecl::powf(HK1, 2) + ecl::powf(vd, 2);
+		const float HK2 = powf(HK0, 2.f) + powf(HK1, 2.f) + powf(vd, 2.f);
 		const float v_tas_pred = sqrtf(HK2); // predicted airspeed
 		//const float HK3 = powf(HK2, -1.0F/2.0F);
 		// calculation can be badly conditioned for very low airspeed values so don't fuse this time

src/modules/ekf2/test/test_EKF_initialization.cpp

@@ -120,13 +120,13 @@ public:
 	void learningCorrectAccelBias()
 	{
 		const Dcmf R_to_earth = Dcmf(_ekf->getQuaternion());
-		const Vector3f dvel_bias_var = _ekf_wrapper.getDeltaVelBiasVariance();
+		const Vector3f accel_bias_var = _ekf->getAccelBiasVariance();
 		const Vector3f accel_bias = _ekf->getAccelBias();
 
 		for (int i = 0; i < 3; i++) {
 			if (fabsf(R_to_earth(2, i)) > 0.8f) {
 				// Highly observable, the variance decreases
-				EXPECT_LT(dvel_bias_var(i), 4.0e-6f) << "axis " << i;
+				EXPECT_LT(accel_bias_var(i), 4.0e-2f) << "axis " << i;
 			}
 
 			EXPECT_LT(accel_bias(i), 4.0e-6f) << "axis " << i;

src/modules/ekf2/test/test_EKF_utils.cpp

@@ -42,8 +42,6 @@
 #include <vector>
 #include <mathlib/mathlib.h>
 
-#include "EKF/utils.hpp"
-
 TEST(eclPowfTest, compareToStandardImplementation)
 {
 	std::vector<int> exponents = {-3, -2, -1, -0, 0, 1, 2, 3};
@@ -51,7 +49,7 @@ TEST(eclPowfTest, compareToStandardImplementation)
 
 	for (auto const exponent : exponents) {
 		for (auto const basis : bases) {
-			EXPECT_EQ(ecl::powf(basis, exponent),
+			EXPECT_EQ(powf(basis, exponent),
 				  std::pow(basis, static_cast<float>(exponent)));
 		}
 	}

src/modules/ekf2/test/test_EKF_yaw_estimator_generated.cpp

@@ -74,9 +74,9 @@ void sympyYawEstUpdate(const SquareMatrix3f &P, float velObsVar, SquareMatrix<fl
 	const float P22 = P(2, 2);
 
 	// optimized auto generated code from SymPy script src/lib/ecl/EKF/python/ekf_derivation/main.py
-	const float t0 = ecl::powf(P01, 2);
+	const float t0 = powf(P01, 2.f);
 	const float t1 = -t0;
-	const float t2 = P00 * P11 + P00 * velObsVar + P11 * velObsVar + t1 + ecl::powf(velObsVar, 2);
+	const float t2 = P00 * P11 + P00 * velObsVar + P11 * velObsVar + t1 + powf(velObsVar, 2.f);
 
 	if (fabsf(t2) < 1e-6f) {
 		return;
@@ -157,9 +157,9 @@ void sympyYawEstPrediction(const Vector3f &state, const SquareMatrix3f &P, const
 	const float dazVar = d_ang_var;
 
 	const float S0 = cosf(psi);
-	const float S1 = ecl::powf(S0, 2);
+	const float S1 = powf(S0, 2.f);
 	const float S2 = sinf(psi);
-	const float S3 = ecl::powf(S2, 2);
+	const float S3 = powf(S2, 2.f);
 	const float S4 = S0 * dvy + S2 * dvx;
 	const float S5 = P02 - P22 * S4;
 	const float S6 = S0 * dvx - S2 * dvy;