diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h
index 42cea5ac9c..4c20f205f3 100644
--- a/src/modules/ekf2/EKF/ekf.h
+++ b/src/modules/ekf2/EKF/ekf.h
@@ -769,20 +769,47 @@ private:
 	// Return the magnetic declination in radians to be used by the alignment and fusion processing
 	float getMagDeclination();
 
-	bool measurementUpdate(Vector24f &K, float innovation_variance, float innovation)
+	void clearInhibitedStateKalmanGains(Vector24f &K) const
 	{
+		// gyro bias: states 10, 11, 12
 		for (unsigned i = 0; i < 3; i++) {
-			// gyro bias: states 10, 11, 12
 			if (_gyro_bias_inhibit[i]) {
-				K(10 + i) = 0.0f;
-			}
-
-			// accel bias: states 13, 14, 15
-			if (_accel_bias_inhibit[i]) {
-				K(13 + i) = 0.0f;
+				K(10 + i) = 0.f;
 			}
 		}
 
+		// accel bias: states 13, 14, 15
+		for (unsigned i = 0; i < 3; i++) {
+			if (_accel_bias_inhibit[i]) {
+				K(13 + i) = 0.f;
+			}
+		}
+
+		// mag I: states 16, 17, 18
+		if (!_control_status.flags.mag_3D) {
+			K(16) = 0.f;
+			K(17) = 0.f;
+			K(18) = 0.f;
+		}
+
+		// mag B: states 19, 20, 21
+		if (!_control_status.flags.mag_3D) {
+			K(19) = 0.f;
+			K(20) = 0.f;
+			K(21) = 0.f;
+		}
+
+		// wind: states 22, 23
+		if (!_control_status.flags.wind) {
+			K(22) = 0.f;
+			K(23) = 0.f;
+		}
+	}
+
+	bool measurementUpdate(Vector24f &K, float innovation_variance, float innovation)
+	{
+		clearInhibitedStateKalmanGains(K);
+
 		const Vector24f KS = K * innovation_variance;
 		SquareMatrix24f KHP;
 
diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp
index 6876c58b0f..77e662e918 100644
--- a/src/modules/ekf2/EKF/mag_fusion.cpp
+++ b/src/modules/ekf2/EKF/mag_fusion.cpp
@@ -258,7 +258,7 @@ bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_so
 	// only calculate gains for states we are using
 	Vector24f Kfusion;
 
-	for (uint8_t row = 0; row <= 15; row++) {
+	for (uint8_t row = 0; row < _k_num_states; row++) {
 		for (uint8_t col = 0; col <= 3; col++) {
 			Kfusion(row) += P(row, col) * H_YAW(col);
 		}
@@ -266,16 +266,6 @@ bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_so
 		Kfusion(row) *= heading_innov_var_inv;
 	}
 
-	if (_control_status.flags.wind) {
-		for (uint8_t row = 22; row <= 23; row++) {
-			for (uint8_t col = 0; col <= 3; col++) {
-				Kfusion(row) += P(row, col) * H_YAW(col);
-			}
-
-			Kfusion(row) *= heading_innov_var_inv;
-		}
-	}
-
 	// define the innovation gate size
 	float gate_sigma = math::max(_params.heading_innov_gate, 1.f);
 
diff --git a/src/modules/ekf2/EKF/vel_pos_fusion.cpp b/src/modules/ekf2/EKF/vel_pos_fusion.cpp
index f44c91650f..17a57518b6 100644
--- a/src/modules/ekf2/EKF/vel_pos_fusion.cpp
+++ b/src/modules/ekf2/EKF/vel_pos_fusion.cpp
@@ -200,17 +200,7 @@ bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o
 		Kfusion(row) = P(row, state_index) / innov_var;
 	}
 
-	for (unsigned i = 0; i < 3; i++) {
-		// gyro bias: states 10, 11, 12
-		if (_gyro_bias_inhibit[i]) {
-			Kfusion(10 + i) = 0.0f;
-		}
-
-		// accel bias: states 13, 14, 15
-		if (_accel_bias_inhibit[i]) {
-			Kfusion(13 + i) = 0.0f;
-		}
-	}
+	clearInhibitedStateKalmanGains(Kfusion);
 
 	SquareMatrix24f KHP;