From dd259a288f3f30f016758fe70634e321bc379a27 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <gncsolns@gmail.com>
Date: Thu, 7 Jan 2021 19:28:09 +1100
Subject: [PATCH] EKF: Don't modify accel bias states and variances when
 inhibited

---
 EKF/ekf.h | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/EKF/ekf.h b/EKF/ekf.h
index 5541732253..2f84e6436d 100644
--- a/EKF/ekf.h
+++ b/EKF/ekf.h
@@ -700,8 +700,14 @@ private:
 	// measurement update with a single measurement
 	// returns true if fusion is performed
 	template <size_t ...Idxs>
-	bool measurementUpdate(const Vector24f &K, const SparseVector24f<Idxs...> &H, float innovation)
+	bool measurementUpdate(Vector24f &K, const SparseVector24f<Idxs...> &H, float innovation)
 	{
+		for (unsigned i = 0; i < 3; i++) {
+			if (_accel_bias_inhibit[i]) {
+				K(13 + i) = 0.0f;
+			}
+		}
+
 		// apply covariance correction via P_new = (I -K*H)*P
 		// first calculate expression for KHP
 		// then calculate P - KHP