From 81c6d6655f6e409cac549ed8537b22996b413028 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Thu, 13 Feb 2020 14:12:26 +0100
Subject: [PATCH] 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        |  2 +-
 EKF/ekf.h              |  2 +-
 EKF/ekf_helper.cpp     | 29 +++++------------------------
 EKF/gps_yaw_fusion.cpp |  2 +-
 4 files changed, 8 insertions(+), 27 deletions(-)

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;