Browse Source

AP_NavEKF3: Fix bug preventing learning of XY IMU dvel bias in flight

gps-1.3.1
Paul Riseborough 4 years ago committed by Andrew Tridgell
parent
commit
ce4d13091a
  1. 2
      libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

2
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@ -1143,7 +1143,7 @@ void NavEKF3_core::CovariancePrediction(Vector3F *rotVarVecPtr) @@ -1143,7 +1143,7 @@ void NavEKF3_core::CovariancePrediction(Vector3F *rotVarVecPtr)
const uint8_t index = stateIndex - 13;
// Don't attempt learning of IMU delta velocty bias if on ground and not aligned with the gravity vector
const bool is_bias_observable = (fabsF(prevTnb[index][2]) > 0.8f) && onGround;
const bool is_bias_observable = (fabsF(prevTnb[index][2]) > 0.8f) || !onGround;
if (!is_bias_observable && !dvelBiasAxisInhibit[index]) {
// store variances to be reinstated wben learning can commence later

Loading…
Cancel
Save