Browse Source

ekf2: Fix bug causing excessive lane selection switching

release/1.12
Paul Riseborough 4 years ago committed by Daniel Agar
parent
commit
3e4c38f6e8
  1. 2
      src/modules/ekf2/EKF2Selector.cpp

2
src/modules/ekf2/EKF2Selector.cpp

@ -281,7 +281,7 @@ bool EKF2Selector::UpdateErrorScores() @@ -281,7 +281,7 @@ bool EKF2Selector::UpdateErrorScores()
const float error_delta = _instance[i].combined_test_ratio - _instance[_selected_instance].combined_test_ratio;
// reduce error only if its better than the primary instance by at least EKF2_SEL_ERR_RED to prevent unnecessary selection changes
const float threshold = _gyro_fault_detected ? fmaxf(_param_ekf2_sel_err_red.get(), 0.05f) : 0.0f;
const float threshold = _gyro_fault_detected ? 0.0f : fmaxf(_param_ekf2_sel_err_red.get(), 0.05f);
if (error_delta > 0 || error_delta < -threshold) {
_instance[i].relative_test_ratio += error_delta;

Loading…
Cancel
Save