ekf: clean uncorrelateQuatStates function

As the name can be ambiguous, it gets renamed
"uncorrelateQuatFromOtherStates".
Also replace the loops storing the values and reapplying them by simply
zeroing two slices (this also saves 130B of flash).

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)));

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

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()

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;