ekf2: update quaternion covariance at yaw reset(#22123)

- Preserve tilt variance while doing a yaw reset
 - Yaw variance is now correctly set instead of increased

---------

Co-authored-by: Dominique <dominique.leblanc@donecle.com>
Co-authored-by: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)>

src/modules/ekf2/EKF/covariance.cpp

@@ -527,15 +527,20 @@ bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrixState &KHP)
 
 void Ekf::resetQuatCov(const float yaw_noise)
 {
-	const float tilt_var = sq(fmaxf(_params.initial_tilt_err, 1.0e-2f));
-	Vector3f rot_var_ned(tilt_var, tilt_var, 0.f);
+	const float tilt_var = sq(math::max(_params.initial_tilt_err, 0.01f));
+	float yaw_var = sq(0.01f);
 
 	// update the yaw angle variance using the variance of the measurement
 	if (PX4_ISFINITE(yaw_noise)) {
 		// using magnetic heading tuning parameter
-		rot_var_ned(2) = (sq(fmaxf(yaw_noise, 1.0e-2f)));
+		yaw_var = math::max(sq(yaw_noise), yaw_var);
 	}
 
+	resetQuatCov(Vector3f(tilt_var, tilt_var, yaw_var));
+}
+
+void Ekf::resetQuatCov(const Vector3f &rot_var_ned)
+{
 	// clear existing quaternion covariance
 	// Optimization: avoid the creation of a <4> function
 	P.uncorrelateCovarianceSetVariance<2>(State::quat_nominal.idx, 0.0f);

src/modules/ekf2/EKF/ekf.h

@@ -1160,7 +1160,8 @@ private:
 	// gravity fusion: heuristically enable / disable gravity fusion
 	void controlGravityFusion(const imuSample &imu_delayed);
 
-	void resetQuatCov(float yaw_noise = NAN);
+	void resetQuatCov(const float yaw_noise = NAN);
+	void resetQuatCov(const Vector3f &euler_noise_ned);
 
 	void resetMagCov();
 
@@ -1168,10 +1169,6 @@ private:
 	void resetWind();
 	void resetWindToZero();
 
-	// Increase the yaw error variance of the quaternions
-	// Argument is additional yaw variance in rad**2
-	void increaseQuatYawErrVariance(float yaw_variance);
-
 	void resetGyroBiasZCov();
 
 	// uncorrelate quaternion states from other states

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -945,19 +945,20 @@ void Ekf::updateGroundEffect()
 }
 #endif // CONFIG_EKF2_BAROMETER
 
-void Ekf::increaseQuatYawErrVariance(float yaw_variance)
-{
-	matrix::SquareMatrix<float, 4> q_cov;
-	sym::RotVarNedToLowerTriangularQuatCov(getStateAtFusionHorizonAsVector(), Vector3f(0.f, 0.f, yaw_variance), &q_cov);
-	q_cov.copyLowerToUpperTriangle();
-	P.slice<4, 4>(0, 0) += q_cov;
-}
-
 void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
 {
 	// save a copy of the quaternion state for later use in calculating the amount of reset change
 	const Quatf quat_before_reset = _state.quat_nominal;
 
+	// save a copy of covariance in NED frame to restore it after the quat reset
+	const matrix::SquareMatrix3f rot_cov = diag(calcRotVecVariances());
+	Vector3f rot_var_ned_before_reset = matrix::SquareMatrix3f(_R_to_earth * rot_cov * _R_to_earth.T()).diag();
+
+	// update the yaw angle variance
+	if (PX4_ISFINITE(yaw_variance) && (yaw_variance > FLT_EPSILON)) {
+		rot_var_ned_before_reset(2) = yaw_variance;
+	}
+
 	// update transformation matrix from body to world frame using the current estimate
 	// update the rotation matrix using the new yaw value
 	_R_to_earth = updateYawInRotMat(yaw, Dcmf(_state.quat_nominal));
@@ -970,10 +971,8 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
 	_state.quat_nominal = quat_after_reset;
 	uncorrelateQuatFromOtherStates();
 
-	// update the yaw angle variance
-	if (PX4_ISFINITE(yaw_variance) && (yaw_variance > FLT_EPSILON)) {
-		increaseQuatYawErrVariance(yaw_variance);
-	}
+	// restore covariance
+	resetQuatCov(rot_var_ned_before_reset);
 
 	// add the reset amount to the output observer buffered data
 	_output_predictor.resetQuaternion(q_error);