ekf2: remove unused shared fields for last velocity observation and variance

src/modules/ekf2/EKF/control.cpp

@@ -318,10 +318,7 @@ void Ekf::controlExternalVisionFusion()
 				resetVelocity();
 			}
 
-			Vector2f ev_vel_innov_gates;
+			_ev_vel_innov = _state.vel - getVisionVelocityInEkfFrame();
-
-			_last_vel_obs = getVisionVelocityInEkfFrame();
-			_ev_vel_innov = _state.vel - _last_vel_obs;
 
 			// check if we have been deadreckoning too long
 			if (isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)) {
@@ -332,12 +329,13 @@ void Ekf::controlExternalVisionFusion()
 				}
 			}
 
-			_last_vel_obs_var = matrix::max(getVisionVelocityVarianceInEkfFrame(), sq(0.05f));
+			const Vector3f obs_var = matrix::max(getVisionVelocityVarianceInEkfFrame(), sq(0.05f));
 
-			ev_vel_innov_gates.setAll(fmaxf(_params.ev_vel_innov_gate, 1.0f));
+			const float innov_gate = fmaxf(_params.ev_vel_innov_gate, 1.f);
+			const Vector2f ev_vel_innov_gates{innov_gate, innov_gate};
 
-			fuseHorizontalVelocity(_ev_vel_innov, ev_vel_innov_gates, _last_vel_obs_var, _ev_vel_innov_var, _ev_vel_test_ratio);
+			fuseHorizontalVelocity(_ev_vel_innov, ev_vel_innov_gates, obs_var, _ev_vel_innov_var, _ev_vel_test_ratio);
-			fuseVerticalVelocity(_ev_vel_innov, ev_vel_innov_gates, _last_vel_obs_var, _ev_vel_innov_var, _ev_vel_test_ratio);
+			fuseVerticalVelocity(_ev_vel_innov, ev_vel_innov_gates, obs_var, _ev_vel_innov_var, _ev_vel_test_ratio);
 		}
 
 		// determine if we should use the yaw observation
@@ -1077,9 +1075,7 @@ void Ekf::controlAuxVelFusion()
 
 				const Vector2f aux_vel_innov_gate(_params.auxvel_gate, _params.auxvel_gate);
 
-				_last_vel_obs = auxvel_sample_delayed.vel;
+				_aux_vel_innov = _state.vel - auxvel_sample_delayed.vel;
-				_aux_vel_innov = _state.vel - _last_vel_obs;
-				_last_vel_obs_var = _aux_vel_innov_var;
 
 				fuseHorizontalVelocity(_aux_vel_innov, aux_vel_innov_gate, auxvel_sample_delayed.velVar,
 						       _aux_vel_innov_var, _aux_vel_test_ratio);

src/modules/ekf2/EKF/ekf.h

@@ -420,9 +420,6 @@ private:
 	Vector3f _delta_vel_bias_var_accum{};		///< kahan summation algorithm accumulator for delta velocity bias variance
 	Vector3f _delta_angle_bias_var_accum{};	///< kahan summation algorithm accumulator for delta angle bias variance
 
-	Vector3f _last_vel_obs{};			///< last velocity observation (m/s)
-	Vector3f _last_vel_obs_var{};		///< last velocity observation variance (m/s)**2
-	Vector2f _last_fail_hvel_innov{};		///< last failed horizontal velocity innovation (m/s)**2
 	float _vert_pos_innov_ratio{0.f};	///< vertical position innovation divided by estimated standard deviation of innovation
 	uint64_t _vert_pos_fuse_attempt_time_us{0};	///< last system time in usec vertical position measurement fuson was attempted
 	float _vert_vel_innov_ratio{0.f};		///< standard deviation of vertical velocity innovation

src/modules/ekf2/EKF/gps_fusion.cpp

@@ -41,8 +41,6 @@
 
 void Ekf::fuseGpsVelPos()
 {
-	Vector2f gps_vel_innov_gates; // [horizontal vertical]
-	Vector2f gps_pos_innov_gates; // [horizontal vertical]
 	Vector3f gps_pos_obs_var;
 
 	// correct velocity for offset relative to IMU
@@ -72,20 +70,21 @@ void Ekf::fuseGpsVelPos()
 
 	_gps_sample_delayed.sacc = fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise);
 
-	_last_vel_obs_var.setAll(sq(_gps_sample_delayed.sacc));
+	const float vel_var = sq(_gps_sample_delayed.sacc);
-	_last_vel_obs_var(2) *= sq(1.5f);
+	const Vector3f gps_vel_obs_var{vel_var, vel_var, vel_var * sq(1.5f)};
 
 	// calculate innovations
-	_last_vel_obs = _gps_sample_delayed.vel;
+	_gps_vel_innov = _state.vel - _gps_sample_delayed.vel;
-	_gps_vel_innov = _state.vel - _last_vel_obs;
 	_gps_pos_innov.xy() = Vector2f(_state.pos) - _gps_sample_delayed.pos;
 
 	// set innovation gate size
-	gps_pos_innov_gates(0) = fmaxf(_params.gps_pos_innov_gate, 1.0f);
+	const Vector2f gps_pos_innov_gates{fmaxf(_params.gps_pos_innov_gate, 1.f), 0.f};
-	gps_vel_innov_gates(0) = gps_vel_innov_gates(1) = fmaxf(_params.gps_vel_innov_gate, 1.0f);
+
+	const float vel_innov_gate = fmaxf(_params.gps_vel_innov_gate, 1.f);
+	const Vector2f gps_vel_innov_gates{vel_innov_gate, vel_innov_gate};
 
 	// fuse GPS measurement
-	fuseHorizontalVelocity(_gps_vel_innov, gps_vel_innov_gates, _last_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio);
+	fuseHorizontalVelocity(_gps_vel_innov, gps_vel_innov_gates, gps_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio);
-	fuseVerticalVelocity(_gps_vel_innov, gps_vel_innov_gates, _last_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio);
+	fuseVerticalVelocity(_gps_vel_innov, gps_vel_innov_gates, gps_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio);
 	fuseHorizontalPosition(_gps_pos_innov, gps_pos_innov_gates, gps_pos_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio);
 }

src/modules/ekf2/EKF/vel_pos_fusion.cpp

@@ -65,8 +65,6 @@ bool Ekf::fuseHorizontalVelocity(const Vector3f &innov, const Vector2f &innov_ga
 		return true;
 
 	} else {
-		_last_fail_hvel_innov(0) = innov(0);
-		_last_fail_hvel_innov(1) = innov(1);
 		_innov_check_fail_status.flags.reject_hor_vel = true;
 		return false;
 	}