dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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.
This commit is contained in:
bresch 2024-03-25 11:53:25 +01:00
parent d9169ba07d
commit 41acb1ba7e
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/ekf2/EKF/covariance.cpp
View File

@ -53,7 +53,7 @@ void Ekf::initialiseCovariance()
{ {
P.zero(); P.zero();
resetQuatCov(); resetQuatCov(0.f); // Start with no initial uncertainty to improve fine leveling through zero vel/pos fusion
// velocity // velocity
#if defined(CONFIG_EKF2_GNSS) #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 // update the yaw angle variance using the variance of the measurement
if (PX4_ISFINITE(yaw_noise)) { if (PX4_ISFINITE(yaw_noise)) {
// using magnetic heading tuning parameter // 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)); resetQuatCov(Vector3f(tilt_var, tilt_var, yaw_var));