From e35737679798a82dec317c0d94b2996a482cfb73 Mon Sep 17 00:00:00 2001
From: kamilritz <kritz@ethz.ch>
Date: Fri, 3 Jul 2020 22:17:13 +0200
Subject: [PATCH] Bring declaration and initialization together

---
 EKF/gps_yaw_fusion.cpp |  5 ++---
 EKF/optflow_fusion.cpp | 23 ++++++++++-------------
 2 files changed, 12 insertions(+), 16 deletions(-)

diff --git a/EKF/gps_yaw_fusion.cpp b/EKF/gps_yaw_fusion.cpp
index 252613fe2d..186b93f728 100644
--- a/EKF/gps_yaw_fusion.cpp
+++ b/EKF/gps_yaw_fusion.cpp
@@ -146,13 +146,10 @@ void Ekf::fuseGpsYaw()
 		_heading_innov_var += H_YAW[row] * PH[row];
 	}
 
-	float heading_innov_var_inv;
-
 	// check if the innovation variance calculation is badly conditioned
 	if (_heading_innov_var >= R_YAW) {
 		// the innovation variance contribution from the state covariances is not negative, no fault
 		_fault_status.flags.bad_hdg = false;
-		heading_innov_var_inv = 1.0f / _heading_innov_var;
 
 	} else {
 		// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
@@ -164,6 +161,8 @@ void Ekf::fuseGpsYaw()
 		return;
 	}
 
+	const float heading_innov_var_inv = 1.f / _heading_innov_var;
+
 	// calculate the Kalman gains
 	// only calculate gains for states we are using
 	Vector24f Kfusion;
diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp
index 91d4296107..07098f1d22 100644
--- a/EKF/optflow_fusion.cpp
+++ b/EKF/optflow_fusion.cpp
@@ -220,19 +220,17 @@ void Ekf::fuseOptFlow()
 			float t87 = t2*t22*t56;
 			float t92 = t2*t7*t69;
 			float t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92;
-			float t78;
 
-			// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
-			if (t77 >= R_LOS) {
-				t78 = 1.0f / t77;
-				_flow_innov_var(0) = t77;
-
-			} else {
+			// protect against a badly conditioned calculation
+			if (t77 < R_LOS) {
 				// we need to reinitialise the covariance matrix and abort this fusion step
 				initialiseCovariance();
 				return;
 			}
 
+			float t78 = 1.0f / t77;
+			_flow_innov_var(0) = t77;
+
 			// calculate Kalman gains for X-axis observation
 			Kfusion[0][0] = t78*(t12-P(0,4)*t2*t7+P(0,1)*t2*t15+P(0,6)*t2*t10+P(0,2)*t2*t19-P(0,3)*t2*t22+P(0,5)*t2*t27);
 			Kfusion[1][0] = t78*(t31+P(1,0)*t2*t5-P(1,4)*t2*t7+P(1,6)*t2*t10+P(1,2)*t2*t19-P(1,3)*t2*t22+P(1,5)*t2*t27);
@@ -363,18 +361,17 @@ void Ekf::fuseOptFlow()
 			float t85 = t2*t19*t49;
 			float t94 = t2*t10*t76;
 			float t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94;
-			float t78;
-			// calculate innovation variance for Y axis observation and protect against a badly conditioned calculation
-			if (t77 >= R_LOS) {
-				t78 = 1.0f / t77;
-				_flow_innov_var(1) = t77;
 
-			} else {
+			// protect against a badly conditioned calculation
+			if (t77 < R_LOS) {
 				// we need to reinitialise the covariance matrix and abort this fusion step
 				initialiseCovariance();
 				return;
 			}
 
+			float t78 = 1.0f / t77;
+			_flow_innov_var(1) = t77;
+
 			// calculate Kalman gains for Y-axis observation
 			Kfusion[0][1] = -t78*(t12+P(0,5)*t2*t8-P(0,6)*t2*t10+P(0,1)*t2*t16-P(0,2)*t2*t19+P(0,3)*t2*t22+P(0,4)*t2*t27);
 			Kfusion[1][1] = -t78*(t31+P(1,0)*t2*t5+P(1,5)*t2*t8-P(1,6)*t2*t10-P(1,2)*t2*t19+P(1,3)*t2*t22+P(1,4)*t2*t27);