From 3222e8f7cbf8e76ba5a6a221b76cc1aec9c0feb3 Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
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 808f9e86b..fa8c3e488 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;
--
GitLab