From 9d7340e187b1f791627a88a7e11b26b1bb57a9f1 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 10 Feb 2016 12:53:24 +1100 Subject: [PATCH] EKF: Fix bugs in position and velocity resets The position reset was not being compensated for velocity and measurement delay The height was being reset with the position. It has been moved into a separate reset function The maximum accepted GPS delay of 100msec was inadequate The states was being incorrectly reset to the GPS position and Baro height on initial alignment. --- EKF/ekf.cpp | 3 ++- EKF/ekf_helper.cpp | 8 +++++++- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 8a9987dfea..32e4b87c47 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -272,9 +272,10 @@ bool Ekf::initialiseFilter(void) // calculate the averaged barometer reading _baro_at_alignment = _baro_sum / (float)_baro_counter; + // set the velocity to the GPS measurement (by definition, the initial position and height is at the origin) resetVelocity(); - resetPosition(); + // initialise the state covariance matrix initialiseCovariance(); return true; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 56f11c3be9..5796cd8628 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -85,7 +85,13 @@ void Ekf::resetHeight() { // if we have a valid height measurement, use it to initialise the vertical position state baroSample baro_newest = _baro_buffer.get_newest(); - _state.pos(2) = _baro_at_alignment - baro_newest.hgt; + + if (_time_last_imu - baro_newest.time_us < 200000) { + _state.pos(2) = _baro_at_alignment - baro_newest.hgt; + + } else { + // XXX use the value of the last known position + } } #if defined(__PX4_POSIX) && !defined(__PX4_QURT)