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); }