mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-27 10:17:45 +08:00
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:
@@ -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));
|
||||||
|
|||||||
Reference in New Issue
Block a user