From 8acfbb2ee07ebefb5a6b17327be0e107b3e6f24b Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sat, 22 Nov 2014 13:26:16 +1100
Subject: [PATCH] AP_NavEKF: add another health check in the EKF
if SV, SP and SH are all off then the most likely cause is divergence
of the EKF. This was done based on a flight log with bad gyro cal
---
libraries/AP_NavEKF/AP_NavEKF.cpp | 6 ++++++
1 file changed, 6 insertions(+)
diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 2346c134d..a02b3a2ba 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -378,6 +378,12 @@ bool NavEKF::healthy(void) const
if (state.velocity.is_nan()) {
return false;
}
+ if (_fallback && velTestRatio > 1 && posTestRatio > 1 && hgtTestRatio > 1) {
+ // all three metrics being above 1 means the filter is
+ // extremely unhealthy.
+ return false;
+ }
+
// If measurements have failed innovation consistency checks for long enough to time-out
// and force fusion then the nav solution can be conidered to be unhealthy
// This will only be set as a transient
--
GitLab