fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: improve tilt leveling speed

Browse Source 
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 committed by Daniel Agar
parent 12fefbcfbd
commit 0c9e4013d2
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();
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));