From f7a53d69f217067ecc84a7d834dc988a35c1dcf6 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 25 Feb 2016 12:51:17 +1100 Subject: [PATCH] EKF: Improve height reset function Add a method to reset the vertical velocity to enable in-flight resets Reset to GPS height if baro height is not available. --- EKF/ekf_helper.cpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 2c72aeb9b9..f126098563 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -83,15 +83,31 @@ void Ekf::resetPosition() // Reset height state using the last baro measurement void Ekf::resetHeight() { + // if we have a valid GPS measurement use it to initialise the vertical velocity state + gpsSample gps_newest = _gps_buffer.get_newest(); + + if (_time_last_imu - gps_newest.time_us < 400000) { + _state.vel(2) = gps_newest.vel(2); + + } else { + _state.vel(2) = 0.0f; + } + // if we have a valid height measurement, use it to initialise the vertical position state baroSample baro_newest = _baro_buffer.get_newest(); if (_time_last_imu - baro_newest.time_us < 200000) { + // use baro as the default _state.pos(2) = _baro_at_alignment - baro_newest.hgt; + } else if (_time_last_imu - gps_newest.time_us < 400000) { + // use GPS as a backup + _state.pos(2) = _gps_alt_ref - gps_newest.hgt; + } else { - // XXX use the value of the last known position + // Do not modify the state as there are no measurements to use } + } // Reset heading and magnetic field states