From 04165a60a7157755649cb4504fc9d08392fd547b Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p-riseborough@live.com.au>
Date: Fri, 6 Nov 2015 12:12:08 +1100
Subject: [PATCH] AP_NavEKF2: Reduce false positive on EKF health check

Apply filtering to baro innovation check and and don't apply innovation checks once aiding has commenced because GPS and baro disturbances on the ground and during launch could generate a false positive
---
 libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp      |  4 ++--
 libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp | 10 ++++++++++
 libraries/AP_NavEKF2/AP_NavEKF2_core.cpp         |  1 +
 libraries/AP_NavEKF2/AP_NavEKF2_core.h           |  1 +
 4 files changed, 14 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
index 109a2ce8c6..805f9a3cce 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
@@ -31,9 +31,9 @@ bool NavEKF2_core::healthy(void) const
     if ((imuSampleTime_ms - ekfStartTime_ms) < 1000 ) {
         return false;
     }
-    // barometer and position innovations must be within limits when on-ground
+    // position and height innovations must be within limits when on-ground and in a static mode of operation
     float horizErrSq = sq(innovVelPos[3]) + sq(innovVelPos[4]);
-    if (onGround && (fabsf(innovVelPos[5]) > 1.0f || horizErrSq > 1.0f)) {
+    if (onGround && (PV_AidingMode == AID_NONE) && ((horizErrSq > 1.0f) || (fabsf(hgtInnovFiltState) > 1.0f))) {
         return false;
     }
 
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
index 93af3e4ddd..e3f85a9c11 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
@@ -410,6 +410,16 @@ void NavEKF2_core::FuseVelPosNED()
             hgtTimeout = (imuSampleTime_ms - lastHgtPassTime_ms) > hgtRetryTime_ms;
             // Fuse height data if healthy or timed out or in constant position mode
             if (hgtHealth || hgtTimeout || (PV_AidingMode == AID_NONE)) {
+                // Calculate a filtered value to be used by pre-flight health checks
+                // We need to filter because wind gusts can generate significant baro noise and we want to be able to detect bias errors in the inertial solution
+                if (onGround) {
+                    float dtBaro = (imuSampleTime_ms - lastHgtPassTime_ms)*1.0e-3f;
+                    const float hgtInnovFiltTC = 2.0f;
+                    float alpha = constrain_float(dtBaro/(dtBaro+hgtInnovFiltTC),0.0f,1.0f);
+                    hgtInnovFiltState += (innovVelPos[5]-hgtInnovFiltState)*alpha;
+                } else {
+                    hgtInnovFiltState = 0.0f;
+                }
                 hgtHealth = true;
                 lastHgtPassTime_ms = imuSampleTime_ms;
                 // if timed out, reset the height, but do not fuse data on this time step
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
index 71e52fd43b..5c8615ef22 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
@@ -203,6 +203,7 @@ void NavEKF2_core::InitialiseVariables()
     magFuseTiltInhibit = false;
     posResetNE.zero();
     velResetNE.zero();
+    hgtInnovFiltState = 0.0f;
 }
 
 // Initialise the states from accelerometer and magnetometer data (if present)
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h
index ffcf144fce..6524fcdcbd 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h
@@ -782,6 +782,7 @@ private:
     float yawTestRatio;             // square of magnetometer yaw angle innovation divided by fail threshold
     Quaternion prevQuatMagReset;    // Quaternion from the last time the magnetic field state reset condition test was performed
     uint8_t fusionHorizonOffset;    // number of IMU samples that the fusion time horizon  has been shifted forward to prevent multiple EKF instances fusing data at the same time
+    float hgtInnovFiltState;        // state used for fitering of the height innovations used for pre-flight checks
 
     // variables used to calulate a vertical velocity that is kinematically consistent with the verical position
     float posDownDerivative;        // Rate of chage of vertical position (dPosD/dt) in m/s. This is the first time derivative of PosD.