From 41acb1ba7e03ee97f592687c4c730362df54c1c8 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 25 Mar 2024 11:53:25 +0100 Subject: [PATCH] ekf2: improve tilt leveling speed Starting with no yaw uncertainty makes the tilt more observable when using fake position fusion during the quasi-stationary alignment phase. --- src/modules/ekf2/EKF/covariance.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 482596070b..055feef795 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -53,7 +53,7 @@ void Ekf::initialiseCovariance() { P.zero(); - resetQuatCov(); + resetQuatCov(0.f); // Start with no initial uncertainty to improve fine leveling through zero vel/pos fusion // velocity #if defined(CONFIG_EKF2_GNSS) @@ -271,7 +271,7 @@ void Ekf::resetQuatCov(const float yaw_noise) // update the yaw angle variance using the variance of the measurement if (PX4_ISFINITE(yaw_noise)) { // using magnetic heading tuning parameter - yaw_var = math::max(sq(yaw_noise), yaw_var); + yaw_var = sq(yaw_noise); } resetQuatCov(Vector3f(tilt_var, tilt_var, yaw_var));