Browse Source

EKF: tighten wind variance growth check

master
Paul Riseborough 7 years ago committed by Daniel Agar
parent
commit
31142c0322
No known key found for this signature in database
GPG Key ID: FD3CBA98017A69DE
  1. 2
      EKF/covariance.cpp

2
EKF/covariance.cpp

@ -210,7 +210,7 @@ void Ekf::predictCovariance() @@ -210,7 +210,7 @@ void Ekf::predictCovariance()
float wind_vel_sig;
// Don't continue to grow wind velocity state variances if they are becoming too large or we are not using wind velocity states as this can make the covariance matrix badly conditioned
if (_control_status.flags.wind && (P[22][22] + P[23][23]) < 2.0f * sq(_params.initial_wind_uncertainty)) {
if (_control_status.flags.wind && (P[22][22] + P[23][23]) < sq(_params.initial_wind_uncertainty)) {
wind_vel_sig = dt * math::constrain(_params.wind_vel_p_noise, 0.0f, 1.0f);
} else {

Loading…
Cancel
Save