From e80fb8b3fa301fb3857d3306099da7a763be99ee Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 6 Jan 2016 19:58:15 +1100 Subject: [PATCH] AP_NavEKF2: Improve non-GPS in-flight attitude accuracy The non-GPS mode was not being activated for small height gains - eg indoor flight. The incorrect innovation consistency check was being applied to the synthetic velocity observations. --- libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp index 3b8fc1434f..aa6af599f4 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp @@ -248,7 +248,7 @@ void NavEKF2_core::FuseVelPosNED() // if the GPS is able to report a speed error, we use it to adjust the observation noise for GPS velocity // otherwise we scale it using manoeuvre acceleration // Use different errors if flying without GPS using synthetic position and velocity data - if (PV_AidingMode == AID_NONE && inFlight) { + if (PV_AidingMode == AID_NONE && motorsArmed) { // Assume the vehicle will be flown with velocity changes less than 10 m/s in this mode (realistic for indoor use) // This is a compromise between corrections for gyro errors and reducing angular errors due to maneouvres R_OBS[0] = sq(10.0f); @@ -257,6 +257,8 @@ void NavEKF2_core::FuseVelPosNED() // Assume a large position uncertainty so as to contrain position states in this mode but minimise angular errors due to manoeuvres R_OBS[3] = sq(25.0f); R_OBS[4] = R_OBS[3]; + R_OBS[5] = posDownObsNoise; + for (uint8_t i=0; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i]; } else { if (gpsSpdAccuracy > 0.0f) { // use GPS receivers reported speed accuracy if available and floor at value set by gps noise parameter @@ -270,14 +272,14 @@ void NavEKF2_core::FuseVelPosNED() R_OBS[1] = R_OBS[0]; R_OBS[3] = sq(constrain_float(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr); R_OBS[4] = R_OBS[3]; + R_OBS[5] = posDownObsNoise; + // For data integrity checks we use the same measurement variances as used to calculate the Kalman gains for all measurements except GPS horizontal velocity + // For horizontal GPs velocity we don't want the acceptance radius to increase with reported GPS accuracy so we use a value based on best GPs perfomrance + // plus a margin for manoeuvres. It is better to reject GPS horizontal velocity errors early + for (uint8_t i=0; i<=2; i++) R_OBS_DATA_CHECKS[i] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag); + for (uint8_t i=3; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i]; } - R_OBS[5] = posDownObsNoise; - // For data integrity checks we use the same measurement variances as used to calculate the Kalman gains for all measurements except GPS horizontal velocity - // For horizontal GPs velocity we don't want the acceptance radius to increase with reported GPS accuracy so we use a value based on best GPs perfomrance - // plus a margin for manoeuvres. It is better to reject GPS horizontal velocity errors early - for (uint8_t i=0; i<=1; i++) R_OBS_DATA_CHECKS[i] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag); - for (uint8_t i=2; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i]; // if vertical GPS velocity data and an independant height source is being used, check to see if the GPS vertical velocity and altimeter // innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting