From 458c67639dbc737a57319ff2a34cd8d33a38515d Mon Sep 17 00:00:00 2001
From: Paul Riseborough <priseborough@sypaq.com.au>
Date: Wed, 22 Sep 2021 07:29:08 +1000
Subject: [PATCH] AP_NavEKF3: Force velocity state to follow GPS when IMU data
 is bad

---
 libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
index e03e58887a..6310134c59 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
@@ -698,9 +698,9 @@ void NavEKF3_core::FuseVelPosNED()
             }
             if (imuSampleTime_ms - badIMUdata_ms < BAD_IMU_DATA_HOLD_MS) {
                 badIMUdata = true;
+                stateStruct.velocity.z = gpsDataDelayed.vel.z;
             } else {
                 badIMUdata = false;
-
             }
         }