AP_NavEKF : Fix bug in Z accel bias update for IMU1

libraries/AP_NavEKF/AP_NavEKF.cpp

@@ -2191,8 +2191,10 @@ void NavEKF::FuseVelPosNED()
                 // attitude, velocity and position corrections are spread across multiple prediction cycles between now
                 // and the anticipated time for the next measurement.
                 // Don't spread quaternion corrections if total angle change across predicted interval is going to exceed 0.1 rad
+                // Don't apply corrections to Z bias state as this has been done already as part of the single IMU calculations
                 bool highRates = ((gpsUpdateCountMax * correctedDelAng.length()) > 0.1f);
                 for (uint8_t i = 0; i<=21; i++) {
+                    if (i != 13) {
                         if ((i <= 3 && highRates) || i >= 10 || constPosMode || constVelMode) {
                             states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex];
                         } else {
@@ -2203,6 +2205,7 @@ void NavEKF::FuseVelPosNED()
                             }
                         }
                     }
+                }
                 state.quat.normalize();
 
                 // update the covariance - take advantage of direct observation of a single state at index = stateIndex to reduce computations