From dcb252cc0552d6f26235f04e98ac3c1f012bb514 Mon Sep 17 00:00:00 2001 From: priseborough Date: Sun, 7 Sep 2014 09:04:19 +1000 Subject: [PATCH] AP_NavEKF : Fix bug in reset of position and velocity states If the inertial solution velocity or position needs to be reset to the GPS, the stored state history for the corresponding states should also be reset. Otherwise the next GPS measurement will be compared to an invalid previous state and will be rejected. The position state should be reset to a GPS position corrected for velocity and measurement latency. This will make a noticeable difference for high speed flight vehicles, eg 11m at 50m/s --- libraries/AP_NavEKF/AP_NavEKF.cpp | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 449bbfe4e1..f9aa846f71 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -440,9 +440,19 @@ void NavEKF::ResetVelocity(void) } else if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) { // read the GPS readGpsData(); - // Set vertical GPS velocity to 0 if mode > 0 (assume 0 if no VZ measurement) - if (_fusionModeGPS >= 1) { - velNED[2] = 0.0f; + // reset horizontal velocity states + if (_fusionModeGPS <= 1) { + states[4] = velNED[0]; // north velocity from blended accel data + states[5] = velNED[1]; // east velocity from blended accel data + states[23] = velNED[0]; // north velocity from IMU1 accel data + states[24] = velNED[1]; // east velocity from IMU1 accel data + states[27] = velNED[0]; // north velocity from IMU2 accel data + states[28] = velNED[1]; // east velocity from IMU2 accel data + // over write stored horizontal velocity states to prevent subsequent GPS measurements from being rejected + for (uint8_t i=0; i<=49; i++){ + storedStates[i].velocity[0] = velNED[0]; + storedStates[i].velocity[1] = velNED[1]; + } } // reset filter velocity states state.velocity = velNED;