diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
index 485a05f6240..d2ee6ed7ce8 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
@@ -1216,60 +1216,22 @@ void NavEKF3_core::FuseVelPosNED()
                         KHP[i][j] = Kfusion[i] * P[stateIndex][j];
                     }
                 }
-                // Check that we are not going to drive any variances negative and skip the update if so
-                bool healthyFusion = true;
-                for (uint8_t i= 0; i<=stateIndexLim; i++) {
-                    if (KHP[i][i] > P[i][i]) {
-                        healthyFusion = false;
-                    }
-                }
-                if (healthyFusion) {
-                    // update the covariance matrix
-                    for (uint8_t i= 0; i<=stateIndexLim; i++) {
-                        for (uint8_t j= 0; j<=stateIndexLim; j++) {
-                            P[i][j] = P[i][j] - KHP[i][j];
-                        }
-                    }
 
-                    // update states and renormalise the quaternions
-                    for (uint8_t i = 0; i<=stateIndexLim; i++) {
-                        statesArray[i] = statesArray[i] - Kfusion[i] * innovVelPos[obsIndex];
-                    }
-                    stateStruct.quat.normalize();
-
-                    // force the covariance matrix to be symmetrical and limit the variances to prevent ill-conditioning.
-                    ForceSymmetry();
-                    ConstrainVariances();
-
-                    // record good fusion status
-                    if (obsIndex == 0) {
-                        faultStatus.bad_nvel = false;
-                    } else if (obsIndex == 1) {
-                        faultStatus.bad_evel = false;
-                    } else if (obsIndex == 2) {
-                        faultStatus.bad_dvel = false;
-                    } else if (obsIndex == 3) {
-                        faultStatus.bad_npos = false;
-                    } else if (obsIndex == 4) {
-                        faultStatus.bad_epos = false;
-                    } else if (obsIndex == 5) {
-                        faultStatus.bad_dpos = false;
-                    }
-                } else {
-                    // record bad fusion status
-                    if (obsIndex == 0) {
-                        faultStatus.bad_nvel = true;
-                    } else if (obsIndex == 1) {
-                        faultStatus.bad_evel = true;
-                    } else if (obsIndex == 2) {
-                        faultStatus.bad_dvel = true;
-                    } else if (obsIndex == 3) {
-                        faultStatus.bad_npos = true;
-                    } else if (obsIndex == 4) {
-                        faultStatus.bad_epos = true;
-                    } else if (obsIndex == 5) {
-                        faultStatus.bad_dpos = true;
-                    }
+                // finish fusion from KHP and Kfusion
+                const bool fault = FinishFusion(innovVelPos[obsIndex]);
+                // record health status
+                if (obsIndex == 0) {
+                    faultStatus.bad_nvel = fault;
+                } else if (obsIndex == 1) {
+                    faultStatus.bad_evel = fault;
+                } else if (obsIndex == 2) {
+                    faultStatus.bad_dvel = fault;
+                } else if (obsIndex == 3) {
+                    faultStatus.bad_npos = fault;
+                } else if (obsIndex == 4) {
+                    faultStatus.bad_epos = fault;
+                } else if (obsIndex == 5) {
+                    faultStatus.bad_dpos = fault;
                 }
             }
         }