From 4263fa9ee422ee2f65e97b81e6020f1799f052d9 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 11 Jul 2024 12:22:26 -0400
Subject: [PATCH] ekf2: stop flow fusion on numerical error

---
 .../ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp    | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp
index 531a7b56d0..1c3d72123f 100644
--- a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp
+++ b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp
@@ -63,6 +63,7 @@ bool Ekf::fuseOptFlow(VectorState &H, const bool update_terrain)
 			// we need to reinitialise the covariance matrix and abort this fusion step
 			ECL_ERR("Opt flow error - covariance reset");
 			initialiseCovariance();
+			stopFlowFusion();
 			return false;
 		}