diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp
index 24aa6fbc5a..cbacdb91cd 100644
--- a/EKF/airspeed_fusion.cpp
+++ b/EKF/airspeed_fusion.cpp
@@ -161,23 +161,22 @@ void Ekf::fuseAirspeed()
 		// apply covariance correction via P_new = (I -K*H)*P
 		// first calculate expression for KHP
 		// then calculate P - KHP
-		float KH[_k_num_states][_k_num_states];
-		for (unsigned row = 0; row < _k_num_states; row++) {
-			KH[row][4] = Kfusion[row] * H_TAS[4];
-			KH[row][5] = Kfusion[row] * H_TAS[5];
-			KH[row][6] = Kfusion[row] * H_TAS[6];
-			KH[row][22] = Kfusion[row] * H_TAS[22];
-			KH[row][23] = Kfusion[row] * H_TAS[23];
-		}
-
 		float KHP[_k_num_states][_k_num_states];
+		float KH[5];
 		for (unsigned row = 0; row < _k_num_states; row++) {
+
+			KH[0] = Kfusion[row] * H_TAS[4];
+			KH[1] = Kfusion[row] * H_TAS[5];
+			KH[2] = Kfusion[row] * H_TAS[6];
+			KH[3] = Kfusion[row] * H_TAS[22];
+			KH[4] = Kfusion[row] * H_TAS[23];
+
 			for (unsigned column = 0; column < _k_num_states; column++) {
-				float tmp = KH[row][4] * P[4][column];
-				tmp += KH[row][5] * P[5][column];
-				tmp += KH[row][6] * P[6][column];
-				tmp += KH[row][22] * P[22][column];
-				tmp += KH[row][23] * P[23][column];
+				float tmp = KH[0] * P[4][column];
+				tmp += KH[1] * P[5][column];
+				tmp += KH[2] * P[6][column];
+				tmp += KH[3] * P[22][column];
+				tmp += KH[4] * P[23][column];
 				KHP[row][column] = tmp;
 			}
 		}
diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp
index f413dbe36d..4e12317d96 100644
--- a/EKF/mag_fusion.cpp
+++ b/EKF/mag_fusion.cpp
@@ -309,31 +309,32 @@ void Ekf::fuseMag()
 		// apply covariance correction via P_new = (I -K*H)*P
 		// first calculate expression for KHP
 		// then calculate P - KHP
-		float KH[_k_num_states][_k_num_states];
-		for (unsigned row = 0; row < _k_num_states; row++) {
-			for (unsigned column = 0; column <= 3; column++) {
-				KH[row][column] = Kfusion[row] * H_MAG[column];
-			}
-
-			for (unsigned column = 16; column <= 21; column++) {
-				KH[row][column] = Kfusion[row] * H_MAG[column];
-			}
-
-		}
-
 		float KHP[_k_num_states][_k_num_states];
+		float KH[10];
 		for (unsigned row = 0; row < _k_num_states; row++) {
+
+			KH[0] = Kfusion[row] * H_MAG[0];
+			KH[1] = Kfusion[row] * H_MAG[1];
+			KH[2] = Kfusion[row] * H_MAG[2];
+			KH[3] = Kfusion[row] * H_MAG[3];
+			KH[4] = Kfusion[row] * H_MAG[16];
+			KH[5] = Kfusion[row] * H_MAG[17];
+			KH[6] = Kfusion[row] * H_MAG[18];
+			KH[7] = Kfusion[row] * H_MAG[19];
+			KH[8] = Kfusion[row] * H_MAG[20];
+			KH[9] = Kfusion[row] * H_MAG[21];
+
 			for (unsigned column = 0; column < _k_num_states; column++) {
-				float tmp = KH[row][0] * P[0][column];
-				tmp += KH[row][1] * P[1][column];
-				tmp += KH[row][2] * P[2][column];
-				tmp += KH[row][3] * P[3][column];
-				tmp += KH[row][16] * P[16][column];
-				tmp += KH[row][17] * P[17][column];
-				tmp += KH[row][18] * P[18][column];
-				tmp += KH[row][19] * P[19][column];
-				tmp += KH[row][20] * P[20][column];
-				tmp += KH[row][21] * P[21][column];
+				float tmp = KH[0] * P[0][column];
+				tmp += KH[1] * P[1][column];
+				tmp += KH[2] * P[2][column];
+				tmp += KH[3] * P[3][column];
+				tmp += KH[4] * P[16][column];
+				tmp += KH[5] * P[17][column];
+				tmp += KH[6] * P[18][column];
+				tmp += KH[7] * P[19][column];
+				tmp += KH[8] * P[20][column];
+				tmp += KH[9] * P[21][column];
 				KHP[row][column] = tmp;
 			}
 		}
@@ -639,20 +640,20 @@ void Ekf::fuseHeading()
 	// apply covariance correction via P_new = (I -K*H)*P
 	// first calculate expression for KHP
 	// then calculate P - KHP
-	float KH[_k_num_states][_k_num_states];
-	for (unsigned row = 0; row < _k_num_states; row++) {
-		for (unsigned column = 0; column <= 3; column++) {
-			KH[row][column] = Kfusion[row] * H_YAW[column];
-		}
-	}
-
 	float KHP[_k_num_states][_k_num_states];
+	float KH[4];
 	for (unsigned row = 0; row < _k_num_states; row++) {
+
+		KH[0] = Kfusion[row] * H_YAW[0];
+		KH[1] = Kfusion[row] * H_YAW[1];
+		KH[2] = Kfusion[row] * H_YAW[2];
+		KH[3] = Kfusion[row] * H_YAW[3];
+
 		for (unsigned column = 0; column < _k_num_states; column++) {
-			float tmp = KH[row][0] * P[0][column];
-			tmp += KH[row][1] * P[1][column];
-			tmp += KH[row][2] * P[2][column];
-			tmp += KH[row][3] * P[3][column];
+			float tmp = KH[0] * P[0][column];
+			tmp += KH[1] * P[1][column];
+			tmp += KH[2] * P[2][column];
+			tmp += KH[3] * P[3][column];
 			KHP[row][column] = tmp;
 		}
 	}
@@ -777,18 +778,16 @@ void Ekf::fuseDeclination()
 	// first calculate expression for KHP
 	// then calculate P - KHP
 	// take advantage of the empty columns in KH to reduce the number of operations
-	float KH[_k_num_states][_k_num_states];
-	for (unsigned row = 0; row < _k_num_states; row++) {
-		for (unsigned column = 16; column <= 17; column++) {
-			KH[row][column] = Kfusion[row] * H_DECL[column];
-		}
-	}
-
 	float KHP[_k_num_states][_k_num_states];
+	float KH[2];
 	for (unsigned row = 0; row < _k_num_states; row++) {
+
+		KH[0] = Kfusion[row] * H_DECL[16];
+		KH[1] = Kfusion[row] * H_DECL[17];
+
 		for (unsigned column = 0; column < _k_num_states; column++) {
-			float tmp = KH[row][16] * P[16][column];
-			tmp += KH[row][17] * P[17][column];
+			float tmp = KH[0] * P[16][column];
+			tmp += KH[1] * P[17][column];
 			KHP[row][column] = tmp;
 		}
 	}
diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp
index dead4dd7ae..ef2afe2db7 100644
--- a/EKF/optflow_fusion.cpp
+++ b/EKF/optflow_fusion.cpp
@@ -433,23 +433,26 @@ void Ekf::fuseOptFlow()
 		// apply covariance correction via P_new = (I -K*H)*P
 		// first calculate expression for KHP
 		// then calculate P - KHP
-		float KH[_k_num_states][_k_num_states];
-		for (unsigned row = 0; row < _k_num_states; row++) {
-			for (unsigned column = 0; column <= 6; column++) {
-				KH[row][column] = gain[row] * H_LOS[obs_index][column];
-			}
-		}
-
 		float KHP[_k_num_states][_k_num_states];
+		float KH[7];
 		for (unsigned row = 0; row < _k_num_states; row++) {
+
+			KH[0] = gain[row] * H_LOS[obs_index][0];
+			KH[1] = gain[row] * H_LOS[obs_index][1];
+			KH[2] = gain[row] * H_LOS[obs_index][2];
+			KH[3] = gain[row] * H_LOS[obs_index][3];
+			KH[4] = gain[row] * H_LOS[obs_index][4];
+			KH[5] = gain[row] * H_LOS[obs_index][5];
+			KH[6] = gain[row] * H_LOS[obs_index][6];
+
 			for (unsigned column = 0; column < _k_num_states; column++) {
-				float tmp = KH[row][0] * P[0][column];
-				tmp += KH[row][1] * P[1][column];
-				tmp += KH[row][2] * P[2][column];
-				tmp += KH[row][3] * P[3][column];
-				tmp += KH[row][4] * P[4][column];
-				tmp += KH[row][5] * P[5][column];
-				tmp += KH[row][6] * P[6][column];
+				float tmp = KH[0] * P[0][column];
+				tmp += KH[1] * P[1][column];
+				tmp += KH[2] * P[2][column];
+				tmp += KH[3] * P[3][column];
+				tmp += KH[4] * P[4][column];
+				tmp += KH[5] * P[5][column];
+				tmp += KH[6] * P[6][column];
 				KHP[row][column] = tmp;
 			}
 		}