From 5d441d65e1d4c7c72bec8e74be68b92dbadcaf28 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 2 Mar 2021 14:58:52 +0100 Subject: [PATCH] Accelerate tilt fine alignment by reducing obs noise when at rest Fine tilt alignment is accomplished by fusing fake position data after a coarse tilt leveling. The observation noise can be reduced if the vehicle is at rest to speed up the process. --- EKF/control.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/EKF/control.cpp b/EKF/control.cpp index f6e1beebca..a3956d855f 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -1304,6 +1304,11 @@ void Ekf::controlFakePosFusion() if (_control_status.flags.in_air && _control_status.flags.tilt_align) { fake_pos_obs_var(0) = fake_pos_obs_var(1) = sq(fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise)); + } else if (_control_status.flags.vehicle_at_rest) { + // Accelerate tilt fine alignment by fusing more + // aggressively when the vehicle is at rest + fake_pos_obs_var(0) = fake_pos_obs_var(1) = sq(0.1f); + } else { fake_pos_obs_var(0) = fake_pos_obs_var(1) = sq(0.5f); }