From f4f108d57d6b8dc842b09a2b73cec10ca9144eeb Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 11 Apr 2016 19:04:43 +1000 Subject: [PATCH 1/3] 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 From 74078cde94c070dbe97dd8d88f08c8a7df6051e7 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 11 Apr 2016 19:40:27 +1000 Subject: [PATCH 2/3] EKF: reset state variance when performing a height reset Set vertical position and velocity variances using known sensor error characteristics if we have reset the states to the sensor readings. --- EKF/ekf_helper.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 454e36c312..0f43e3c17c 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -95,6 +95,7 @@ void Ekf::resetHeight() if (_time_last_imu - range_newest.time_us < 2 * RNG_MAX_INTERVAL) { _state.pos(2) = _hgt_sensor_offset - range_newest.rng; + P[8][8] = sq(_params.range_noise); } else { // TODO: reset to last known range based estimate @@ -110,6 +111,7 @@ void Ekf::resetHeight() if (_time_last_imu - baro_newest.time_us < 2 * BARO_MAX_INTERVAL) { _state.pos(2) = _hgt_sensor_offset - baro_newest.hgt + _baro_hgt_offset; + P[8][8] = sq(_params.baro_noise); } else { // TODO: reset to last known baro based estimate @@ -119,6 +121,7 @@ void Ekf::resetHeight() // initialize vertical position and velocity with newest gps measurement if (_time_last_imu - gps_newest.time_us < 2 * GPS_MAX_INTERVAL) { _state.pos(2) = _hgt_sensor_offset - gps_newest.hgt + _gps_alt_ref; + P[8][8] = sq(gps_newest.hacc); } else { // TODO: reset to last known gps based estimate @@ -132,7 +135,9 @@ void Ekf::resetHeight() // 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); + P[5][5] = sq(1.5f * gps_newest.sacc); } else { + P[5][5] = fminf(sq(_state.vel(2)),1000.0f); _state.vel(2) = 0.0f; } From 006b6b58e443a75d7146b478d45d33fb7373552e Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 11 Apr 2016 19:40:27 +1000 Subject: [PATCH 3/3] EKF: fix bug in status print statement --- EKF/control.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index f3287adc5d..8fc114a045 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -184,9 +184,7 @@ void Ekf::controlFusionModes() _control_status.flags.rng_hgt = false; // adjust the height offset so we can use the GPS _hgt_sensor_offset = _state.pos(2) + gps_init.hgt - _gps_alt_ref; - if (!baro_hgt_available) { - printf("EKF baro hgt timeout - switching to gps\n"); - } + printf("EKF baro hgt timeout - switching to gps\n"); } }