diff --git a/EKF/control.cpp b/EKF/control.cpp index 098cd8f368..f84d719429 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -656,7 +656,7 @@ void Ekf::controlHeightSensorTimeouts() const baroSample& baro_init = _baro_buffer.get_newest(); bool baro_data_fresh = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset); - bool baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P[8][8]) * sq(_params.baro_innov_gate); + bool baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P[9][9]) * sq(_params.baro_innov_gate); // if baro data is acceptable and GPS data is inaccurate, reset height to baro bool reset_to_baro = baro_data_consistent && baro_data_fresh && !_baro_hgt_faulty && !gps_hgt_accurate;