diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
index 0f695260a7..9651b3b6e3 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
@@ -506,6 +506,8 @@ void NavEKF3_core::FuseMagnetometer()
             H_MAG[17] = 2.0f*q0*q3 + 2.0f*q1*q2;
             H_MAG[18] = 2.0f*q1*q3 - 2.0f*q0*q2;
             H_MAG[19] = 1.0f;
+            H_MAG[20] = 0.0f;
+            H_MAG[21] = 0.0f;
 
             // calculate Kalman gain
             SK_MX[0] = 1.0f / varInnovMag[0];
@@ -580,7 +582,9 @@ void NavEKF3_core::FuseMagnetometer()
             H_MAG[16] = 2.0f*q1*q2 - 2.0f*q0*q3;
             H_MAG[17] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6];
             H_MAG[18] = 2.0f*q0*q1 + 2.0f*q2*q3;
+            H_MAG[19] = 0.0f;
             H_MAG[20] = 1.0f;
+            H_MAG[21] = 0.0f;
 
             // calculate Kalman gain
             SK_MY[0] = 1.0f / varInnovMag[1];
@@ -656,6 +660,8 @@ void NavEKF3_core::FuseMagnetometer()
             H_MAG[16] = 2.0f*q0*q2 + 2.0f*q1*q3;
             H_MAG[17] = 2.0f*q2*q3 - 2.0f*q0*q1;
             H_MAG[18] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
+            H_MAG[19] = 0.0f;
+            H_MAG[20] = 0.0f;
             H_MAG[21] = 1.0f;
 
             // calculate Kalman gain