From 3222e8f7cbf8e76ba5a6a221b76cc1aec9c0feb3 Mon Sep 17 00:00:00 2001 From: priseborough Date: Sat, 17 May 2014 16:55:22 +1000 Subject: [PATCH] AP_NavEKF: Default parameter adjustments Bring Plane glitch protection thresholds into alignment with copter and rover Slight increase in accelerometer bias process noise to prevent bias estimate divergence into limits (Rover and Plane only as Copter does not seem respond as well to this change) effective increase in threshold on divergence test to allow increased margin for bad GPS velocities --- libraries/AP_NavEKF/AP_NavEKF.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 808f9e86ba..fa8c3e4883 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -56,7 +56,7 @@ #define GYRO_PNOISE_DEFAULT 0.015f #define ACC_PNOISE_DEFAULT 0.25f #define GBIAS_PNOISE_DEFAULT 1E-06f -#define ABIAS_PNOISE_DEFAULT 0.0001f +#define ABIAS_PNOISE_DEFAULT 0.0002f #define MAGE_PNOISE_DEFAULT 0.0003f #define MAGB_PNOISE_DEFAULT 0.0003f #define VEL_GATE_DEFAULT 2 @@ -77,16 +77,16 @@ #define GYRO_PNOISE_DEFAULT 0.015f #define ACC_PNOISE_DEFAULT 0.25f #define GBIAS_PNOISE_DEFAULT 1E-06f -#define ABIAS_PNOISE_DEFAULT 0.0001f +#define ABIAS_PNOISE_DEFAULT 0.0002f #define MAGE_PNOISE_DEFAULT 0.0003f #define MAGB_PNOISE_DEFAULT 0.0003f -#define VEL_GATE_DEFAULT 3 +#define VEL_GATE_DEFAULT 2 #define POS_GATE_DEFAULT 10 #define HGT_GATE_DEFAULT 10 #define MAG_GATE_DEFAULT 3 #define MAG_CAL_DEFAULT 0 #define GLITCH_ACCEL_DEFAULT 150 -#define GLITCH_RADIUS_DEFAULT 50 +#define GLITCH_RADIUS_DEFAULT 15 #endif // APM_BUILD_DIRECTORY @@ -3400,7 +3400,7 @@ void NavEKF::checkDivergence() float tempLength = tempVec.length(); if (tempLength != 0.0f) { float temp = constrain_float((P[10][10] + P[11][11] + P[12][12]),1e-12f,1e-8f); - scaledDeltaGyrBiasLgth = (2e-6f / temp) * tempVec.length() / dtIMU; + scaledDeltaGyrBiasLgth = (1e-6f / temp) * tempVec.length() / dtIMU; } bool divergenceDetected = (scaledDeltaGyrBiasLgth > 1.0f); lastGyroBias = state.gyro_bias;