ekf2: relax zero velocity update

The ZVU is too strong and prevents the EKF from following variations in
the height aiding sources, creating large innovations.
This commit is contained in:
bresch
2022-08-30 16:37:12 +02:00
committed by Daniel Agar
parent d996af4647
commit a2a5093881
@@ -51,7 +51,9 @@ void Ekf::controlZeroVelocityUpdate()
Vector3f vel_obs{0, 0, 0};
Vector3f innovation = _state.vel - vel_obs;
const float obs_var = sq(0.001f);
// Set a low variance initially for faster accel bias learning and higher
// later to let the states follow the measurements
const float obs_var = _NED_origin_initialised ? sq(0.2f) : sq(0.001f);
Vector3f innov_var{
P(4, 4) + obs_var,
P(5, 5) + obs_var,