From a19015ed61e9b52cf9c1379889f16239f31e3c42 Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Fri, 16 May 2014 21:13:30 +1000
Subject: [PATCH] AP_NavEKF: Scale divergence check with covariance
This provides consistent behaviour for a range of gyro bias process
noise settings and automatically adjusts sensitivity as filter learns bias
---
libraries/AP_NavEKF/AP_NavEKF.cpp | 3 ++-
1 file changed, 2 insertions(+), 1 deletion(-)
diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 0b49a5138..13f304e1e 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -3398,7 +3398,8 @@ void NavEKF::checkDivergence()
Vector3f tempVec = state.gyro_bias - lastGyroBias;
float tempLength = tempVec.length();
if (tempLength != 0.0f) {
- scaledDeltaGyrBiasLgth = 5e4f*tempVec.length()/dtIMU;
+ float temp = constrain_float((P[10][10] + P[11][11] + P[12][12]),1e-12f,1e-8f);
+ scaledDeltaGyrBiasLgth = (2e-6f / temp) * tempVec.length() / dtIMU;
}
bool divergenceDetected = (scaledDeltaGyrBiasLgth > 1.0f);
lastGyroBias = state.gyro_bias;
--
GitLab