From f4f108d57d6b8dc842b09a2b73cec10ca9144eeb Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 11 Apr 2016 19:04:43 +1000 Subject: [PATCH] EKF: Reset vertical velocity when performing a height reset --- EKF/ekf_helper.cpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 371366a731..454e36c312 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -87,6 +87,9 @@ bool Ekf::resetPosition() // Reset height state using the last height measurement void Ekf::resetHeight() { + // Get the most recent GPS data + gpsSample gps_newest = _gps_buffer.get_newest(); + if (_control_status.flags.rng_hgt) { rangeSample range_newest = _range_buffer.get_newest(); @@ -114,11 +117,8 @@ void Ekf::resetHeight() } else if (_control_status.flags.gps_hgt) { // initialize vertical position and velocity with newest gps measurement - gpsSample gps_newest = _gps_buffer.get_newest(); - if (_time_last_imu - gps_newest.time_us < 2 * GPS_MAX_INTERVAL) { _state.pos(2) = _hgt_sensor_offset - gps_newest.hgt + _gps_alt_ref; - _state.vel(2) = gps_newest.vel(2); } else { // TODO: reset to last known gps based estimate @@ -129,6 +129,13 @@ void Ekf::resetHeight() _baro_hgt_offset = baro_newest.hgt + _state.pos(2); } + // If we are using GPS, then use it to reset the vertical velocity + if (_control_status.flags.gps && (_time_last_imu - gps_newest.time_us < 2 * GPS_MAX_INTERVAL)) { + _state.vel(2) = gps_newest.vel(2); + } else { + _state.vel(2) = 0.0f; + } + } // Reset heading and magnetic field states