ekf2: move EV vel to new state machine, introduce EKF2_EV_CTRL param

src/modules/ekf2/EKF/CMakeLists.txt

@@ -43,6 +43,7 @@ add_library(ecl_EKF
 	EKFGSF_yaw.cpp
 	estimator_interface.cpp
 	ev_height_control.cpp
+	ev_vel_control.cpp
 	fake_height_control.cpp
 	fake_pos_control.cpp
 	gnss_height_control.cpp

src/modules/ekf2/EKF/common.h

@@ -79,8 +79,9 @@ using math::Utilities::updateYawInRotMat;
 #define GNDEFFECT_TIMEOUT       10E6    ///< Maximum period of time that ground effect protection will be active after it was last turned on (uSec)
 
 enum class VelocityFrame : uint8_t {
-	LOCAL_FRAME_FRD = 0,
-	BODY_FRAME_FRD  = 1
+	LOCAL_FRAME_NED = 0,
+	LOCAL_FRAME_FRD = 1,
+	BODY_FRAME_FRD  = 2
 };
 
 enum GeoDeclinationMask : uint8_t {
@@ -126,6 +127,13 @@ enum RngCtrl : uint8_t {
 	ENABLED     = 2
 };
 
+enum class EvCtrl : uint8_t {
+	HPOS = (1<<0),
+	VPOS = (1<<1),
+	VEL  = (1<<2),
+	YAW  = (1<<3)
+};
+
 enum SensorFusionMask : uint16_t {
 	// Bit locations for fusion_mode
 	DEPRECATED_USE_GPS = (1<<0),    ///< set to true to use GPS data (DEPRECATED, use gnss_ctrl)
@@ -136,7 +144,7 @@ enum SensorFusionMask : uint16_t {
 	USE_DRAG         = (1<<5),      ///< set to true to use the multi-rotor drag model to estimate wind
 	ROTATE_EXT_VIS   = (1<<6),      ///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used
 	DEPRECATED_USE_GPS_YAW = (1<<7),///< set to true to use GPS yaw data if available (DEPRECATED, use gnss_ctrl)
-	USE_EXT_VIS_VEL  = (1<<8),      ///< set to true to use external vision velocity data
+	DEPRECATED_USE_EXT_VIS_VEL = (1<<8), ///< set to true to use external vision velocity data
 };
 
 struct gpsMessage {
@@ -227,8 +235,8 @@ struct extVisionSample {
 	Vector3f    vel{};         ///< FRD velocity in reference frame defined in vel_frame variable (m/sec) - Z must be aligned with down axis
 	Quatf       quat{};        ///< quaternion defining rotation from body to earth frame
 	Vector3f    posVar{};      ///< XYZ position variances (m**2)
-	Vector3f    velVar{};      ///< XYZ velocity variances ((m/sec)**2)
-	float       angVar{};      ///< angular heading variance (rad**2)
+	Vector3f    velocity_var{};    ///< XYZ velocity variances ((m/sec)**2)
+	Vector3f    orientation_var{}; ///< orientation variance (rad**2)
 	VelocityFrame vel_frame = VelocityFrame::BODY_FRAME_FRD;
 	uint8_t     reset_counter{};
 	int8_t     quality{};     ///< quality indicator between 0 and 100
@@ -266,6 +274,7 @@ struct parameters {
 	int32_t baro_ctrl{1};
 	int32_t gnss_ctrl{GnssCtrl::HPOS | GnssCtrl::VEL};
 	int32_t rng_ctrl{RngCtrl::CONDITIONAL};
+	int32_t ev_ctrl{0};
 	int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder |
 				    TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator
 
@@ -358,6 +367,8 @@ struct parameters {
 	float range_kin_consistency_gate{1.0f}; ///< gate size used by the range finder kinematic consistency check
 
 	// vision position fusion
+	float ev_vel_noise{0.1f};               ///< minimum allowed observation noise for EV velocity fusion (m/sec)
+	float ev_att_noise{0.1f};               ///< minimum allowed observation noise for EV attitude fusion (rad/sec)
 	int32_t ev_quality_minimum{0};          ///< vision minimum acceptable quality integer
 	float ev_vel_innov_gate{3.0f};          ///< vision velocity fusion innovation consistency gate size (STD)
 	float ev_pos_innov_gate{5.0f};          ///< vision position fusion innovation consistency gate size (STD)

src/modules/ekf2/EKF/control.cpp

@@ -217,7 +217,7 @@ void Ekf::controlExternalVisionFusion()
 		// if the ev data is not in a NED reference frame, then the transformation between EV and EKF navigation frames
 		// needs to be calculated and the observations rotated into the EKF frame of reference
 		if ((_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) && ((_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_POS)
-				|| (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_VEL)) && !_control_status.flags.ev_yaw) {
+				|| (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))) && !_control_status.flags.ev_yaw) {
 
 			// rotate EV measurements into the EKF Navigation frame
 			calcExtVisRotMat();
@@ -232,11 +232,6 @@ void Ekf::controlExternalVisionFusion()
 				if (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_POS && !_control_status.flags.ev_pos) {
 					startEvPosFusion();
 				}
-
-				// turn on use of external vision measurements for velocity
-				if (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_VEL && !_control_status.flags.ev_vel) {
-					startEvVelFusion();
-				}
 			}
 		}
 
@@ -331,15 +326,6 @@ void Ekf::controlExternalVisionFusion()
 
 				// check if we have been deadreckoning too long
 				if (isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)) {
-					// only reset velocity if we have no another source of aiding constraining it
-					if (isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)1E6) &&
-					    isTimedOut(_time_last_hor_vel_fuse, (uint64_t)1E6)) {
-
-						if (_control_status.flags.ev_vel) {
-							resetVelocityToVision();
-						}
-					}
-
 					resetHorizontalPositionToVision();
 				}
 			}
@@ -354,38 +340,10 @@ void Ekf::controlExternalVisionFusion()
 			fuseHorizontalPosition(_aid_src_ev_pos);
 		}
 
-		// determine if we should use the velocity observations
-		if (_control_status.flags.ev_vel) {
-			if (reset && _control_status_prev.flags.ev_vel) {
-				resetVelocityToVision();
-			}
-
-			// check if we have been deadreckoning too long
-			if (isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)) {
-				// only reset velocity if we have no another source of aiding constraining it
-				if (isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)1E6) &&
-				    isTimedOut(_time_last_hor_pos_fuse, (uint64_t)1E6)) {
-					resetVelocityToVision();
-				}
-			}
-
-			resetEstimatorAidStatus(_aid_src_ev_vel);
-
-			const Vector3f obs_var = matrix::max(getVisionVelocityVarianceInEkfFrame(), sq(0.05f));
-
-			const float innov_gate = fmaxf(_params.ev_vel_innov_gate, 1.f);
-
-			updateVelocityAidSrcStatus(_ev_sample_delayed.time_us, getVisionVelocityInEkfFrame(), obs_var, innov_gate, _aid_src_ev_vel);
-
-			_aid_src_ev_vel.fusion_enabled = true;
-
-			fuseVelocity(_aid_src_ev_vel);
-		}
-
 		// determine if we should use the yaw observation
 		resetEstimatorAidStatus(_aid_src_ev_yaw);
 		const float measured_hdg = getEulerYaw(_ev_sample_delayed.quat);
-		const float ev_yaw_obs_var = fmaxf(_ev_sample_delayed.angVar, 1.e-4f);
+		const float ev_yaw_obs_var = fmaxf(_ev_sample_delayed.orientation_var(2), 1.e-4f);
 
 		if (PX4_ISFINITE(measured_hdg)) {
 			_aid_src_ev_yaw.timestamp_sample = _ev_sample_delayed.time_us;
@@ -414,12 +372,28 @@ void Ekf::controlExternalVisionFusion()
 			}
 		}
 
+
+		bool ev_reset = (_ev_sample_delayed.reset_counter != _ev_sample_delayed_prev.reset_counter);
+
+		// determine if we should use the horizontal position observations
+		bool quality_sufficient = (_params.ev_quality_minimum <= 0) || (_ev_sample_delayed.quality >= _params.ev_quality_minimum);
+
+		bool starting_conditions_passing = quality_sufficient
+						   && ((_ev_sample_delayed.time_us - _ev_sample_delayed_prev.time_us) < EV_MAX_INTERVAL)
+						   && ((_params.ev_quality_minimum <= 0) || (_ev_sample_delayed_prev.quality >= _params.ev_quality_minimum)) // previous quality sufficient
+						   && ((_params.ev_quality_minimum <= 0) || (_ext_vision_buffer->get_newest().quality >= _params.ev_quality_minimum)) // newest quality sufficient
+						   && isNewestSampleRecent(_time_last_ext_vision_buffer_push, EV_MAX_INTERVAL);
+
+		// EV velocity
+		controlEvVelFusion(_ev_sample_delayed, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel);
+
+
 		// record observation and estimate for use next time
 		_ev_sample_delayed_prev = _ev_sample_delayed;
 		_hpos_pred_prev = _state.pos.xy();
 		_yaw_pred_prev = getEulerYaw(_state.quat_nominal);
 
-	} else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel ||  _control_status.flags.ev_yaw)
+	} else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.ev_yaw)
 		   && !isRecent(_ev_sample_delayed.time_us, (uint64_t)_params.reset_timeout_max)) {
 
 		// Turn off EV fusion mode if no data has been received

src/modules/ekf2/EKF/ekf.h

@@ -504,6 +504,8 @@ private:
 	uint64_t _time_last_healthy_rng_data{0};
 	uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
 
+	uint8_t _nb_ev_vel_reset_available{0};
+
 	Vector3f _last_known_pos{};		///< last known local position vector (m)
 
 	uint64_t _time_acc_bias_check{0};	///< last time the  accel bias check passed (uSec)
@@ -700,7 +702,6 @@ private:
 	void resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f &new_horz_vel_var);
 	void resetHorizontalVelocityTo(const Vector2f &new_horz_vel, float vel_var) { resetHorizontalVelocityTo(new_horz_vel, Vector2f(vel_var, vel_var)); }
 
-	void resetVelocityToVision();
 	void resetHorizontalVelocityToZero();
 
 	void resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var);
@@ -784,10 +785,6 @@ private:
 	// update the rotation matrix which transforms EV navigation frame measurements into NED
 	void calcExtVisRotMat();
 
-	Vector3f getVisionVelocityInEkfFrame() const;
-
-	Vector3f getVisionVelocityVarianceInEkfFrame() const;
-
 	// matrix vector multiplication for computing K<24,1> * H<1,24> * P<24,24>
 	// that is optimized by exploring the sparsity in H
 	template <size_t ...Idxs>
@@ -872,6 +869,7 @@ private:
 
 	// control fusion of external vision observations
 	void controlExternalVisionFusion();
+	void controlEvVelFusion(const extVisionSample &ev_sample, bool starting_conditions_passing, bool ev_reset, bool quality_sufficient, estimator_aid_source3d_s& aid_src);
 
 	// control fusion of optical flow observations
 	void controlOpticalFlowFusion();
@@ -1033,7 +1031,6 @@ private:
 	void stopGpsYawFusion();
 
 	void startEvPosFusion();
-	void startEvVelFusion();
 	void startEvYawFusion();
 
 	void stopEvFusion();

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -44,13 +44,6 @@
 #include <mathlib/mathlib.h>
 #include <cstdlib>
 
-void Ekf::resetVelocityToVision()
-{
-	_information_events.flags.reset_vel_to_vision = true;
-	ECL_INFO("reset to vision velocity");
-	resetVelocityTo(getVisionVelocityInEkfFrame(), getVisionVelocityVarianceInEkfFrame());
-}
-
 void Ekf::resetHorizontalVelocityToZero()
 {
 	_information_events.flags.reset_vel_to_zero = true;
@@ -297,7 +290,7 @@ bool Ekf::resetMagHeading()
 bool Ekf::resetYawToEv()
 {
 	const float yaw_new = getEulerYaw(_ev_sample_delayed.quat);
-	const float yaw_new_variance = fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f));
+	const float yaw_new_variance = fmaxf(_ev_sample_delayed.orientation_var(2), sq(1.0e-2f));
 
 	resetQuatStateYaw(yaw_new, yaw_new_variance);
 	_R_ev_to_ekf.setIdentity();
@@ -1156,56 +1149,6 @@ void Ekf::updateGroundEffect()
 	}
 }
 
-Vector3f Ekf::getVisionVelocityInEkfFrame() const
-{
-	Vector3f vel;
-	// correct velocity for offset relative to IMU
-	const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
-	const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
-
-	// rotate measurement into correct earth frame if required
-	switch (_ev_sample_delayed.vel_frame) {
-	case VelocityFrame::BODY_FRAME_FRD:
-		vel = _R_to_earth * (_ev_sample_delayed.vel - vel_offset_body);
-		break;
-
-	case VelocityFrame::LOCAL_FRAME_FRD:
-		const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
-
-		if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) {
-			vel = _R_ev_to_ekf * _ev_sample_delayed.vel - vel_offset_earth;
-
-		} else {
-			vel = _ev_sample_delayed.vel - vel_offset_earth;
-		}
-
-		break;
-	}
-
-	return vel;
-}
-
-Vector3f Ekf::getVisionVelocityVarianceInEkfFrame() const
-{
-	Matrix3f ev_vel_cov = matrix::diag(_ev_sample_delayed.velVar);
-
-	// rotate measurement into correct earth frame if required
-	switch (_ev_sample_delayed.vel_frame) {
-	case VelocityFrame::BODY_FRAME_FRD:
-		ev_vel_cov = _R_to_earth * ev_vel_cov * _R_to_earth.transpose();
-		break;
-
-	case VelocityFrame::LOCAL_FRAME_FRD:
-		if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) {
-			ev_vel_cov = _R_ev_to_ekf * ev_vel_cov * _R_ev_to_ekf.transpose();
-		}
-
-		break;
-	}
-
-	return ev_vel_cov.diag();
-}
-
 // update the rotation matrix which rotates EV measurements into the EKF's navigation frame
 void Ekf::calcExtVisRotMat()
 {
@@ -1368,14 +1311,6 @@ void Ekf::startEvPosFusion()
 	ECL_INFO("starting vision pos fusion");
 }
 
-void Ekf::startEvVelFusion()
-{
-	_control_status.flags.ev_vel = true;
-	resetVelocityToVision();
-	_information_events.flags.starting_vision_vel_fusion = true;
-	ECL_INFO("starting vision vel fusion");
-}
-
 void Ekf::startEvYawFusion()
 {
 	// turn on fusion of external vision yaw measurements and disable all magnetometer fusion
@@ -1405,15 +1340,6 @@ void Ekf::stopEvPosFusion()
 	}
 }
 
-void Ekf::stopEvVelFusion()
-{
-	if (_control_status.flags.ev_vel) {
-		ECL_INFO("stopping EV vel fusion");
-		_control_status.flags.ev_vel = false;
-		resetEstimatorAidStatus(_aid_src_ev_vel);
-	}
-}
-
 void Ekf::stopEvYawFusion()
 {
 	if (_control_status.flags.ev_yaw) {

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -496,7 +496,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 		max_time_delay_ms = math::max(_params.flow_delay_ms, max_time_delay_ms);
 	}
 
-	if (_params.fusion_mode & (SensorFusionMask::USE_EXT_VIS_POS | SensorFusionMask::USE_EXT_VIS_YAW | SensorFusionMask::USE_EXT_VIS_VEL)) {
+	if ((_params.fusion_mode & (SensorFusionMask::USE_EXT_VIS_POS | SensorFusionMask::USE_EXT_VIS_YAW)) || (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))) {
 		max_time_delay_ms = math::max(_params.ev_delay_ms, max_time_delay_ms);
 	}
 

src/modules/ekf2/EKF/ev_height_control.cpp

@@ -94,8 +94,28 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample)
 					bias_est.setBias(-_state.pos(2) + measurement);
 
 					// reset vertical velocity
-					if (PX4_ISFINITE(ev_sample.vel(2)) && (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_POS)) {
-						resetVerticalVelocityTo(ev_sample.vel(2), ev_sample.velVar(2));
+					if (ev_sample.vel.isAllFinite() && (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))) {
+
+						// correct velocity for offset relative to IMU
+						const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
+						const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
+						const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
+
+						switch (ev_sample.vel_frame) {
+						case VelocityFrame::LOCAL_FRAME_NED:
+						case VelocityFrame::LOCAL_FRAME_FRD: {
+								const Vector3f reset_vel = ev_sample.vel - vel_offset_earth;
+								resetVerticalVelocityTo(reset_vel(2), math::max(ev_sample.velocity_var(2), sq(_params.ev_vel_noise)));
+							}
+							break;
+
+						case VelocityFrame::BODY_FRAME_FRD: {
+								const Vector3f reset_vel = _R_to_earth * (ev_sample.vel - vel_offset_body);
+								const Matrix3f reset_vel_cov = _R_to_earth * matrix::diag(ev_sample.velocity_var) * _R_to_earth.transpose();
+								resetVerticalVelocityTo(reset_vel(2), math::max(reset_vel_cov(2, 2), sq(_params.ev_vel_noise)));
+							}
+							break;
+						}
 
 					} else {
 						resetVerticalVelocityToZero();

src/modules/ekf2/EKF/ev_vel_control.cpp

@@ -0,0 +1,231 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ev_vel_control.cpp
+ * Control functions for ekf external vision velocity fusion
+ */
+
+#include "ekf.h"
+
+void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, bool starting_conditions_passing, bool ev_reset,
+			     bool quality_sufficient, estimator_aid_source3d_s &aid_src)
+{
+	static constexpr const char *AID_SRC_NAME = "EV velocity";
+
+	const bool yaw_alignment_changed = (!_control_status_prev.flags.ev_yaw && _control_status.flags.ev_yaw)
+					   || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);
+
+	// determine if we should use EV velocity aiding
+	bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))
+					     && _control_status.flags.tilt_align
+					     && ev_sample.vel.isAllFinite();
+
+	// correct velocity for offset relative to IMU
+	const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
+	const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
+	const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
+
+	// rotate measurement into correct earth frame if required
+	Vector3f vel{NAN, NAN, NAN};
+	Matrix3f vel_cov{};
+
+	switch (ev_sample.vel_frame) {
+	case VelocityFrame::LOCAL_FRAME_NED:
+		if (_control_status.flags.yaw_align) {
+			vel = ev_sample.vel - vel_offset_earth;
+			vel_cov = matrix::diag(ev_sample.velocity_var);
+
+		} else {
+			continuing_conditions_passing = false;
+		}
+
+		break;
+
+	case VelocityFrame::LOCAL_FRAME_FRD:
+		if (_control_status.flags.ev_yaw) {
+			// using EV frame
+			vel = ev_sample.vel - vel_offset_earth;
+			vel_cov = matrix::diag(ev_sample.velocity_var);
+
+		} else {
+			// rotate EV to the EKF reference frame
+			const Quatf q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized());
+			const Dcmf R_ev_to_ekf = Dcmf(q_error);
+
+			vel = R_ev_to_ekf * ev_sample.vel - vel_offset_earth;
+			vel_cov = R_ev_to_ekf * matrix::diag(ev_sample.velocity_var) * R_ev_to_ekf.transpose();
+
+			// increase minimum variance to include EV orientation variance
+			// TODO: do this properly
+			const float orientation_var_max = ev_sample.orientation_var.max();
+
+			for (int i = 0; i < 2; i++) {
+				vel_cov(i, i) = math::max(vel_cov(i, i), orientation_var_max);
+			}
+		}
+
+		break;
+
+	case VelocityFrame::BODY_FRAME_FRD:
+		vel = _R_to_earth * (ev_sample.vel - vel_offset_body);
+		vel_cov = _R_to_earth * matrix::diag(ev_sample.velocity_var) * _R_to_earth.transpose();
+		break;
+
+	default:
+		continuing_conditions_passing = false;
+		break;
+	}
+
+	// increase minimum variance if GPS active (position reference)
+	if (_control_status.flags.gps) {
+		for (int i = 0; i < 2; i++) {
+			vel_cov(i, i) = math::max(vel_cov(i, i), sq(_params.gps_vel_noise));
+		}
+	}
+
+	const Vector3f measurement{vel};
+
+	const Vector3f measurement_var{
+		math::max(vel_cov(0, 0), sq(_params.ev_vel_noise), sq(0.01f)),
+		math::max(vel_cov(1, 1), sq(_params.ev_vel_noise), sq(0.01f)),
+		math::max(vel_cov(2, 2), sq(_params.ev_vel_noise), sq(0.01f))
+	};
+
+	const bool measurement_valid = measurement.isAllFinite() && measurement_var.isAllFinite();
+
+	updateVelocityAidSrcStatus(ev_sample.time_us,
+				   measurement,                               // observation
+				   measurement_var,                           // observation variance
+				   math::max(_params.ev_vel_innov_gate, 1.f), // innovation gate
+				   aid_src);
+
+	if (!measurement_valid) {
+		continuing_conditions_passing = false;
+	}
+
+	starting_conditions_passing &= continuing_conditions_passing
+				       && ((Vector3f(aid_src.test_ratio).max() < 0.1f) || !isHorizontalAidingActive());
+
+	if (_control_status.flags.ev_vel) {
+		aid_src.fusion_enabled = true;
+
+		if (continuing_conditions_passing) {
+
+			if ((ev_reset && isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.ev_vel)) || yaw_alignment_changed) {
+
+				if (quality_sufficient) {
+					ECL_INFO("reset to %s", AID_SRC_NAME);
+					_information_events.flags.reset_vel_to_vision = true;
+					resetVelocityTo(measurement, measurement_var);
+					aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+
+				} else {
+					// EV has reset, but quality isn't sufficient
+					// we have no choice but to stop EV and try to resume once quality is acceptable
+					stopEvVelFusion();
+					return;
+				}
+
+			} else if (quality_sufficient) {
+				fuseVelocity(aid_src);
+
+			} else {
+				aid_src.innovation_rejected = true;
+			}
+
+			const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.no_aid_timeout_max); // 1 second
+
+			if (is_fusion_failing) {
+
+				if ((_nb_ev_vel_reset_available > 0) && quality_sufficient) {
+					// Data seems good, attempt a reset
+					_information_events.flags.reset_vel_to_vision = true;
+					ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME);
+					resetVelocityTo(measurement, measurement_var);
+					aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+
+					if (_control_status.flags.in_air) {
+						_nb_ev_vel_reset_available--;
+					}
+
+				} else if (starting_conditions_passing) {
+					// Data seems good, but previous reset did not fix the issue
+					// something else must be wrong, declare the sensor faulty and stop the fusion
+					//_control_status.flags.ev_vel_fault = true;
+					ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME);
+					stopEvVelFusion();
+
+				} else {
+					// A reset did not fix the issue but all the starting checks are not passing
+					// This could be a temporary issue, stop the fusion without declaring the sensor faulty
+					ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME);
+					stopEvVelFusion();
+				}
+			}
+
+		} else {
+			// Stop fusion but do not declare it faulty
+			ECL_WARN("stopping %s fusion, continuing conditions failing", AID_SRC_NAME);
+			stopEvVelFusion();
+		}
+
+	} else {
+		if (starting_conditions_passing) {
+			// activate fusion, only reset if necessary
+			if (!isHorizontalAidingActive() || yaw_alignment_changed) {
+				ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME);
+				_information_events.flags.reset_vel_to_vision = true;
+				resetVelocityTo(measurement, measurement_var);
+
+			} else {
+				ECL_INFO("starting %s fusion", AID_SRC_NAME);
+			}
+
+			aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+
+			_nb_ev_vel_reset_available = 5;
+			_information_events.flags.starting_vision_vel_fusion = true;
+			_control_status.flags.ev_vel = true;
+		}
+	}
+}
+
+void Ekf::stopEvVelFusion()
+{
+	if (_control_status.flags.ev_vel) {
+		resetEstimatorAidStatus(_aid_src_ev_vel);
+
+		_control_status.flags.ev_vel = false;
+	}
+}