From a2d6a4dded81020671700970784f0bdb5b450af2 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 29 Jan 2018 08:09:55 +1100 Subject: [PATCH] EKF: fix indexing bug (#387) Should use index 9 for vertical position. --- EKF/control.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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;