diff --git a/EKF/control.cpp b/EKF/control.cpp
index d7f8fb3279..4e75e23eec 100644
--- a/EKF/control.cpp
+++ b/EKF/control.cpp
@@ -227,7 +227,7 @@ void Ekf::controlExternalVisionFusion()
 
 				// calculate initial quaternion states for the ekf
 				_state.quat_nominal = Quatf(euler_init);
-				uncorrelateQuatStates();
+				uncorrelateQuatFromOtherStates();
 
 				// adjust the quaternion covariances estimated yaw error
 				increaseQuatYawErrVariance(fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f)));
diff --git a/EKF/ekf.h b/EKF/ekf.h
index 272f1cfbba..8120f31a29 100644
--- a/EKF/ekf.h
+++ b/EKF/ekf.h
@@ -774,7 +774,7 @@ private:
 	void zeroMagCov();
 
 	// uncorrelate quaternion states from other states
-	void uncorrelateQuatStates();
+	void uncorrelateQuatFromOtherStates();
 
 	// Use Kahan summation algorithm to get the sum of "sum_previous" and "input".
 	// This function relies on the caller to be responsible for keeping a copy of
diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp
index a626c83182..00ad25062c 100644
--- a/EKF/ekf_helper.cpp
+++ b/EKF/ekf_helper.cpp
@@ -457,7 +457,7 @@ bool Ekf::realignYawGPS()
 			// calculate new filter quaternion states using corrected yaw angle
 			_state.quat_nominal = Quatf(euler321);
 			_R_to_earth = Dcmf(_state.quat_nominal);
-			uncorrelateQuatStates();
+			uncorrelateQuatFromOtherStates();
 
 			// If heading was bad, then we also need to reset the velocity and position states
 			_velpos_reset_request = badMagYaw;
@@ -691,7 +691,7 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
 	// update quaternion states
 	_state.quat_nominal = quat_after_reset;
 	_R_to_earth = Dcmf(_state.quat_nominal);
-	uncorrelateQuatStates();
+	uncorrelateQuatFromOtherStates();
 
 	// record the state change
 	_state_reset_status.quat_change = q_error;
@@ -1368,29 +1368,10 @@ void Ekf::fuse(float *K, float innovation)
 	}
 }
 
-
-
-void Ekf::uncorrelateQuatStates()
+void Ekf::uncorrelateQuatFromOtherStates()
 {
-	// save 4x4 elements
-	uint32_t row;
-	uint32_t col;
-	float variances[4][4];
-	for (row = 0; row < 4; row++) {
-		for (col = 0; col < 4; col++) {
-			variances[row][col] = P(row, col);
-		}
-	}
-
-	// zero rows and columns
-	P.uncorrelateCovariance<4>(0);
-
-	// restore 4x4 elements
-	for (row = 0; row < 4; row++) {
-		for (col = 0; col < 4; col++) {
-			P(row, col) = variances[row][col];
-		}
-	}
+	P.slice<_k_num_states - 4, 4>(4, 0) = 0.f;	
+	P.slice<4, _k_num_states - 4>(0, 4) = 0.f;	
 }
 
 bool Ekf::global_position_is_valid()
diff --git a/EKF/gps_yaw_fusion.cpp b/EKF/gps_yaw_fusion.cpp
index 271d817420..f7c2d3ef4a 100644
--- a/EKF/gps_yaw_fusion.cpp
+++ b/EKF/gps_yaw_fusion.cpp
@@ -385,7 +385,7 @@ bool Ekf::resetGpsAntYaw()
 		if (delta_ang_error.norm() > math::radians(15.0f) || !_control_status.flags.yaw_align) {
 			// update quaternion states
 			_state.quat_nominal = quat_after_reset;
-			uncorrelateQuatStates();
+			uncorrelateQuatFromOtherStates();
 
 			// record the state change
 			_state_reset_status.quat_change = q_error;