ekf2: EV overhaul yaw and position fusion (#20501)

- move EV yaw and EV position to new state machines
 - EV yaw and EV pos now configured via EKF2_EV_CTRL (migrated from EKF2_AID_MASK)
 - new EV position offset estimator to enable EV position while GPS position is active (no more EV pos delta fusion)
 - yaw_align now strictly means north (no more rotate external vision aid mask)
 - automatic switching between EV yaw, and yaw align north based on GPS quality

msg/CMakeLists.txt

@@ -74,6 +74,7 @@ set(msg_files
 	EstimatorAidSource2d.msg
 	EstimatorAidSource3d.msg
 	EstimatorBias.msg
+	EstimatorBias3d.msg
 	EstimatorEventFlags.msg
 	EstimatorGpsStatus.msg
 	EstimatorInnovations.msg

msg/EstimatorBias.msg

@@ -9,4 +9,4 @@ float32 innov			# innovation of the last measurement fusion (m)
 float32 innov_var		# innovation variance of the last measurement fusion (m^2)
 float32 innov_test_ratio	# normalized innovation squared test ratio
 
-# TOPICS estimator_baro_bias estimator_gnss_hgt_bias estimator_rng_hgt_bias estimator_ev_hgt_bias
+# TOPICS estimator_baro_bias estimator_gnss_hgt_bias estimator_rng_hgt_bias

msg/EstimatorBias3d.msg

@@ -0,0 +1,14 @@
+uint64 timestamp                # time since system start (microseconds)
+uint64 timestamp_sample         # the timestamp of the raw data (microseconds)
+
+uint32 device_id                # unique device ID for the sensor that does not change between power cycles
+
+float32[3] bias                 # estimated barometric altitude bias (m)
+float32[3] bias_var             # estimated barometric altitude bias variance (m^2)
+
+float32[3] innov                # innovation of the last measurement fusion (m)
+float32[3] innov_var            # innovation variance of the last measurement fusion (m^2)
+float32[3] innov_test_ratio     # normalized innovation squared test ratio
+
+# TOPICS estimator_bias3d
+# TOPICS estimator_ev_pos_bias

msg/VehicleOdometry.msg

@@ -28,4 +28,4 @@ uint8 reset_counter
 int8 quality
 
 # TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry
-# TOPICS estimator_odometry estimator_visual_odometry_aligned
+# TOPICS estimator_odometry

src/modules/ekf2/CMakeLists.txt

@@ -91,8 +91,11 @@ px4_add_module(
 		EKF/ekf_helper.cpp
 		EKF/EKFGSF_yaw.cpp
 		EKF/estimator_interface.cpp
+		EKF/ev_control.cpp
 		EKF/ev_height_control.cpp
+		EKF/ev_pos_control.cpp
 		EKF/ev_vel_control.cpp
+		EKF/ev_yaw_control.cpp
 		EKF/fake_height_control.cpp
 		EKF/fake_pos_control.cpp
 		EKF/gnss_height_control.cpp

src/modules/ekf2/EKF/CMakeLists.txt

@@ -42,8 +42,11 @@ add_library(ecl_EKF
 	ekf_helper.cpp
 	EKFGSF_yaw.cpp
 	estimator_interface.cpp
+	ev_control.cpp
 	ev_height_control.cpp
+	ev_pos_control.cpp
 	ev_vel_control.cpp
+	ev_yaw_control.cpp
 	fake_height_control.cpp
 	fake_pos_control.cpp
 	gnss_height_control.cpp

src/modules/ekf2/EKF/bias_estimator.hpp

@@ -64,7 +64,9 @@ public:
 		float innov_test_ratio{INFINITY};
 	};
 
+	BiasEstimator() {}
 	BiasEstimator(float state_init, float state_var_init): _state{state_init}, _state_var{state_var_init} {};
+
 	virtual ~BiasEstimator() = default;
 
 	void reset()

src/modules/ekf2/EKF/common.h

@@ -79,6 +79,11 @@ static constexpr float BADACC_BIAS_PNOISE = 4.9f;  ///< The delta velocity proce
 // ground effect compensation
 static constexpr uint64_t GNDEFFECT_TIMEOUT = 10e6; ///< Maximum period of time that ground effect protection will be active after it was last turned on (uSec)
 
+enum class PositionFrame : uint8_t {
+	LOCAL_FRAME_NED = 0,
+	LOCAL_FRAME_FRD = 1,
+};
+
 enum class VelocityFrame : uint8_t {
 	LOCAL_FRAME_NED = 0,
 	LOCAL_FRAME_FRD = 1,
@@ -115,6 +120,12 @@ enum HeightSensor : uint8_t {
 	UNKNOWN  = 4
 };
 
+enum class PositionSensor : uint8_t {
+	UNKNOWN = 0,
+	GNSS    = 1,
+	EV      = 2,
+};
+
 enum GnssCtrl : uint8_t {
 	HPOS  = (1<<0),
 	VPOS  = (1<<1),
@@ -140,11 +151,11 @@ enum SensorFusionMask : uint16_t {
 	DEPRECATED_USE_GPS = (1<<0),    ///< set to true to use GPS data (DEPRECATED, use gnss_ctrl)
 	USE_OPT_FLOW     = (1<<1),      ///< set to true to use optical flow data
 	INHIBIT_ACC_BIAS = (1<<2),      ///< set to true to inhibit estimation of accelerometer delta velocity bias
-	USE_EXT_VIS_POS  = (1<<3),      ///< set to true to use external vision position data
+	DEPRECATED_USE_EXT_VIS_POS = (1<<3), ///< set to true to use external vision position data
-	USE_EXT_VIS_YAW  = (1<<4),      ///< set to true to use external vision quaternion data for yaw
+	DEPRECATED_USE_EXT_VIS_YAW = (1<<4), ///< set to true to use external vision quaternion data for yaw
 	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_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)
+	DEPRECATED_USE_GPS_YAW     = (1<<7), ///< set to true to use GPS yaw data if available (DEPRECATED, use gnss_ctrl)
 	DEPRECATED_USE_EXT_VIS_VEL = (1<<8), ///< set to true to use external vision velocity data
 };
 
@@ -238,9 +249,10 @@ struct extVisionSample {
 	Vector3f    pos{};         ///< XYZ position in external vision's local reference frame (m) - Z must be aligned with down axis
 	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    position_var{};    ///< XYZ position variances (m**2)
 	Vector3f    velocity_var{};    ///< XYZ velocity variances ((m/sec)**2)
 	Vector3f    orientation_var{}; ///< orientation variance (rad**2)
+	PositionFrame pos_frame = PositionFrame::LOCAL_FRAME_FRD;
 	VelocityFrame vel_frame = VelocityFrame::BODY_FRAME_FRD;
 	uint8_t     reset_counter{};
 	int8_t     quality{};     ///< quality indicator between 0 and 100
@@ -283,6 +295,7 @@ struct parameters {
 	// measurement source control
 	int32_t fusion_mode{};         ///< bitmasked integer that selects some aiding sources
 	int32_t height_sensor_ref{HeightSensor::BARO};
+	int32_t position_sensor_ref{static_cast<int32_t>(PositionSensor::GNSS)};
 	int32_t baro_ctrl{1};
 	int32_t gnss_ctrl{GnssCtrl::HPOS | GnssCtrl::VEL};
 	int32_t rng_ctrl{RngCtrl::CONDITIONAL};
@@ -325,7 +338,7 @@ struct parameters {
 	const float initial_wind_uncertainty{1.0f};     ///< 1-sigma initial uncertainty in wind velocity (m/sec)
 
 	// position and velocity fusion
-	float gps_vel_noise{5.0e-1f};           ///< minimum allowed observation noise for gps velocity fusion (m/sec)
+	float gps_vel_noise{0.5f};           ///< minimum allowed observation noise for gps velocity fusion (m/sec)
 	float gps_pos_noise{0.5f};              ///< minimum allowed observation noise for gps position fusion (m)
 	float gps_hgt_bias_nsd{0.13f};          ///< process noise for gnss height bias estimation (m/s/sqrt(Hz))
 	float pos_noaid_noise{10.0f};           ///< observation noise for non-aiding position fusion (m)

src/modules/ekf2/EKF/control.cpp

@@ -183,10 +183,6 @@ void Ekf::controlFusionModes()
 		}
 	}
 
-	if (_ext_vision_buffer) {
-		_ev_data_ready = _ext_vision_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed);
-	}
-
 	if (_airspeed_buffer) {
 		_tas_data_ready = _airspeed_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed);
 	}
@@ -203,7 +199,7 @@ void Ekf::controlFusionModes()
 	controlDragFusion();
 	controlHeightFusion();
 
-	// Additional data odoemtery data from an external estimator can be fused.
+	// Additional data odometry data from an external estimator can be fused.
 	controlExternalVisionFusion();
 
 	// Additional horizontal velocity data from an auxiliary sensor can be fused
@@ -221,213 +217,6 @@ void Ekf::controlFusionModes()
 	updateDeadReckoningStatus();
 }
 
-void Ekf::controlExternalVisionFusion()
-{
-	// Check for new external vision data
-	if (_ev_data_ready) {
-
-		bool reset = false;
-
-		if (_ev_sample_delayed.reset_counter != _ev_sample_delayed_prev.reset_counter) {
-			reset = true;
-		}
-
-		if (_inhibit_ev_yaw_use) {
-			stopEvYawFusion();
-		}
-
-		// 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.ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))) && !_control_status.flags.ev_yaw) {
-
-			// rotate EV measurements into the EKF Navigation frame
-			calcExtVisRotMat();
-		}
-
-		// external vision aiding selection logic
-		if (_control_status.flags.tilt_align && _control_status.flags.yaw_align) {
-
-			// check for a external vision measurement that has fallen behind the fusion time horizon
-			if (isNewestSampleRecent(_time_last_ext_vision_buffer_push, 2 * EV_MAX_INTERVAL)) {
-				// turn on use of external vision measurements for position
-				if (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_POS && !_control_status.flags.ev_pos) {
-					startEvPosFusion();
-				}
-			}
-		}
-
-		// external vision yaw aiding selection logic
-		if (!_inhibit_ev_yaw_use && (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_YAW) && !_control_status.flags.ev_yaw
-		    && _control_status.flags.tilt_align) {
-
-			// don't start using EV data unless data is arriving frequently
-			if (isNewestSampleRecent(_time_last_ext_vision_buffer_push, 2 * EV_MAX_INTERVAL)) {
-				if (resetYawToEv()) {
-					_control_status.flags.yaw_align = true;
-					startEvYawFusion();
-				}
-			}
-		}
-
-		// determine if we should use the horizontal position observations
-		if (_control_status.flags.ev_pos) {
-			resetEstimatorAidStatus(_aid_src_ev_pos);
-
-			if (reset && _control_status_prev.flags.ev_pos) {
-				if (!_fuse_hpos_as_odom) {
-					resetHorizontalPositionToVision();
-				}
-			}
-
-			Vector3f ev_pos_obs_var;
-
-			// correct position and height for offset relative to IMU
-			const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
-			const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
-			_ev_sample_delayed.pos -= pos_offset_earth;
-
-			// Use an incremental position fusion method for EV position data if GPS is also used
-			if (_params.gnss_ctrl & GnssCtrl::HPOS) {
-				_fuse_hpos_as_odom = true;
-
-			} else {
-				_fuse_hpos_as_odom = false;
-			}
-
-			if (_fuse_hpos_as_odom) {
-				if (!_hpos_prev_available) {
-					// no previous observation available to calculate position change
-					_hpos_prev_available = true;
-
-				} else {
-					// calculate the change in position since the last measurement
-					// rotate measurement into body frame is required when fusing with GPS
-					Vector3f ev_delta_pos = _R_ev_to_ekf * Vector3f(_ev_sample_delayed.pos - _ev_sample_delayed_prev.pos);
-
-					// use the change in position since the last measurement
-					_aid_src_ev_pos.observation[0] = ev_delta_pos(0);
-					_aid_src_ev_pos.observation[1] = ev_delta_pos(1);
-
-					_aid_src_ev_pos.innovation[0] = _state.pos(0) - _hpos_pred_prev(0) - ev_delta_pos(0);
-					_aid_src_ev_pos.innovation[1] = _state.pos(1) - _hpos_pred_prev(1) - ev_delta_pos(1);
-
-					// observation 1-STD error, incremental pos observation is expected to have more uncertainty
-					Matrix3f ev_pos_var = matrix::diag(_ev_sample_delayed.posVar);
-					ev_pos_var = _R_ev_to_ekf * ev_pos_var * _R_ev_to_ekf.transpose();
-					ev_pos_obs_var(0) = fmaxf(ev_pos_var(0, 0), sq(0.5f));
-					ev_pos_obs_var(1) = fmaxf(ev_pos_var(1, 1), sq(0.5f));
-
-					_aid_src_ev_pos.observation_variance[0] = ev_pos_obs_var(0);
-					_aid_src_ev_pos.observation_variance[1] = ev_pos_obs_var(1);
-
-					_aid_src_ev_pos.innovation_variance[0] = P(7, 7) + _aid_src_ev_pos.observation_variance[0];
-					_aid_src_ev_pos.innovation_variance[1] = P(8, 8) + _aid_src_ev_pos.observation_variance[1];
-				}
-			} else {
-				// use the absolute position
-				Vector3f ev_pos_meas = _ev_sample_delayed.pos;
-				Matrix3f ev_pos_var = matrix::diag(_ev_sample_delayed.posVar);
-
-				if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) {
-					ev_pos_meas = _R_ev_to_ekf * ev_pos_meas;
-					ev_pos_var = _R_ev_to_ekf * ev_pos_var * _R_ev_to_ekf.transpose();
-				}
-
-				_aid_src_ev_pos.observation[0] = ev_pos_meas(0);
-				_aid_src_ev_pos.observation[1] = ev_pos_meas(1);
-
-				_aid_src_ev_pos.observation_variance[0] = fmaxf(ev_pos_var(0, 0), sq(0.01f));
-				_aid_src_ev_pos.observation_variance[1] = fmaxf(ev_pos_var(1, 1), sq(0.01f));
-
-				_aid_src_ev_pos.innovation[0] = _state.pos(0) - _aid_src_ev_pos.observation[0];
-				_aid_src_ev_pos.innovation[1] = _state.pos(1) - _aid_src_ev_pos.observation[1];
-
-				_aid_src_ev_pos.innovation_variance[0] = P(7, 7) + _aid_src_ev_pos.observation_variance[0];
-				_aid_src_ev_pos.innovation_variance[1] = P(8, 8) + _aid_src_ev_pos.observation_variance[1];
-
-				// check if we have been deadreckoning too long
-				if (isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)) {
-					resetHorizontalPositionToVision();
-				}
-			}
-
-			// innovation gate size
-			const float ev_pos_innov_gate = fmaxf(_params.ev_pos_innov_gate, 1.0f);
-			setEstimatorAidStatusTestRatio(_aid_src_ev_pos, ev_pos_innov_gate);
-
-			_aid_src_ev_pos.timestamp_sample = _ev_sample_delayed.time_us;
-			_aid_src_ev_pos.fusion_enabled = true;
-
-			fuseHorizontalPosition(_aid_src_ev_pos);
-		}
-
-		// 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.orientation_var(2), 1.e-4f);
-
-		if (PX4_ISFINITE(measured_hdg)) {
-			_aid_src_ev_yaw.timestamp_sample = _ev_sample_delayed.time_us;
-			_aid_src_ev_yaw.observation = measured_hdg;
-			_aid_src_ev_yaw.observation_variance = ev_yaw_obs_var;
-			_aid_src_ev_yaw.fusion_enabled = _control_status.flags.ev_yaw;
-
-			if (_control_status.flags.ev_yaw) {
-				if (reset && _control_status_prev.flags.ev_yaw) {
-					resetYawToEv();
-				}
-
-				const float innovation = wrap_pi(getEulerYaw(_R_to_earth) - measured_hdg);
-
-				fuseYaw(innovation, ev_yaw_obs_var, _aid_src_ev_yaw);
-
-			} else {
-				// populate estimator_aid_src_ev_yaw with delta heading innovations for logging
-				// use the change in yaw since the last measurement
-				const float measured_hdg_prev = getEulerYaw(_ev_sample_delayed_prev.quat);
-
-				// calculate the change in yaw since the last measurement
-				const float ev_delta_yaw = wrap_pi(measured_hdg - measured_hdg_prev);
-
-				_aid_src_ev_yaw.innovation = wrap_pi(getEulerYaw(_R_to_earth) - _yaw_pred_prev - ev_delta_yaw);
-			}
-		}
-
-
-		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);
-
-		const 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);
-
-		// EV height
-		controlEvHeightFusion(_ev_sample_delayed, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_hgt);
-
-
-		// 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 || _control_status.flags.ev_hgt)
-		   && !isRecent(_ev_sample_delayed.time_us, (uint64_t)_params.reset_timeout_max)) {
-
-		// Turn off EV fusion mode if no data has been received
-		stopEvFusion();
-		_warning_events.flags.vision_data_stopped = true;
-		ECL_WARN("vision data stopped");
-	}
-}
-
 void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing)
 {
 	if (!(_params.gnss_ctrl & GnssCtrl::YAW)

src/modules/ekf2/EKF/ekf.h

@@ -48,6 +48,7 @@
 #include "EKFGSF_yaw.h"
 #include "bias_estimator.hpp"
 #include "height_bias_estimator.hpp"
+#include "position_bias_estimator.hpp"
 
 #include <uORB/topics/estimator_aid_source1d.h>
 #include <uORB/topics/estimator_aid_source2d.h>
@@ -379,9 +380,6 @@ public:
 	// return a bitmask integer that describes which state estimates can be used for flight control
 	void get_ekf_soln_status(uint16_t *status) const;
 
-	// return the quaternion defining the rotation from the External Vision to the EKF reference frame
-	matrix::Quatf getVisionAlignmentQuaternion() const { return Quatf(_R_ev_to_ekf); };
-
 	// use the latest IMU data at the current time horizon.
 	Quatf calculate_quaternion() const;
 
@@ -410,6 +408,8 @@ public:
 	const BiasEstimator::status &getRngHgtBiasEstimatorStatus() const { return _rng_hgt_b_est.getStatus(); }
 	const BiasEstimator::status &getEvHgtBiasEstimatorStatus() const { return _ev_hgt_b_est.getStatus(); }
 
+	const BiasEstimator::status &getEvPosBiasEstimatorStatus(int i) const { return _ev_pos_b_est.getStatus(i); }
+
 	const auto &aid_src_airspeed() const { return _aid_src_airspeed; }
 	const auto &aid_src_sideslip() const { return _aid_src_sideslip; }
 
@@ -476,23 +476,18 @@ private:
 
 	bool _filter_initialised{false};	///< true when the EKF sttes and covariances been initialised
 
-	// variables used when position data is being fused using a relative position odometry model
+	float _ev_yaw_pred_prev{};                 ///< previous value of yaw state used by odometry fusion (m)
-	bool _fuse_hpos_as_odom{false};		///< true when the NE position data is being fused using an odometry assumption
-	Vector2f _hpos_pred_prev{};		///< previous value of NE position state used by odometry fusion (m)
-	float _yaw_pred_prev{};                 ///< previous value of yaw state used by odometry fusion (m)
-	bool _hpos_prev_available{false};	///< true when previous values of the estimate and measurement are available for use
-	Dcmf _R_ev_to_ekf;			///< transformation matrix that rotates observations from the EV to the EKF navigation frame, initialized with Identity
 	bool _inhibit_ev_yaw_use{false};	///< true when the vision yaw data should not be used (e.g.: NE fusion requires true North)
 
 	// booleans true when fresh sensor data is available at the fusion time horizon
 	bool _gps_data_ready{false};	///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
 	bool _rng_data_ready{false};
 	bool _flow_data_ready{false};	///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon
-	bool _ev_data_ready{false};	///< true when new external vision system data has fallen behind the fusion time horizon and is available to be fused
 	bool _tas_data_ready{false};	///< true when new true airspeed data has fallen behind the fusion time horizon and is available to be fused
 	bool _flow_for_terrain_data_ready{false}; /// same flag as "_flow_data_ready" but used for separate terrain estimator
 
 	uint64_t _time_prev_gps_us{0};	///< time stamp of previous GPS data retrieved from the buffer (uSec)
+
 	uint64_t _time_last_horizontal_aiding{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec)
 	uint64_t _time_last_v_pos_aiding{0};
 	uint64_t _time_last_v_vel_aiding{0};
@@ -508,7 +503,9 @@ 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_pos_reset_available{0};
 	uint8_t _nb_ev_vel_reset_available{0};
+	uint8_t _nb_ev_yaw_reset_available{0};
 
 	Vector3f _last_known_pos{};		///< last known local position vector (m)
 
@@ -708,7 +705,6 @@ private:
 	void resetHorizontalVelocityToZero();
 
 	void resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var);
-	void resetHorizontalPositionToVision();
 	void resetHorizontalPositionToLastKnown();
 
 	void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var);
@@ -776,19 +772,12 @@ private:
 	// return true if successful
 	bool resetMagHeading();
 
-	// reset the heading using the external vision measurements
-	// return true if successful
-	bool resetYawToEv();
-
 	// Return the magnetic declination in radians to be used by the alignment and fusion processing
 	float getMagDeclination();
 
 	// modify output filter to match the the EKF state at the fusion time horizon
 	void alignOutputFilter();
 
-	// update the rotation matrix which transforms EV navigation frame measurements into NED
-	void calcExtVisRotMat();
-
 	bool measurementUpdate(Vector24f &K, float innovation_variance, float innovation)
 	{
 		for (unsigned i = 0; i < 3; i++) {
@@ -851,8 +840,16 @@ private:
 
 	// control fusion of external vision observations
 	void controlExternalVisionFusion();
+
 	void controlEvHeightFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s& aid_src);
+
+	void controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s& aid_src);
+	void startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, estimator_aid_source2d_s &aid_src);
+	void updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, bool reset, estimator_aid_source2d_s &aid_src);
+	void stopEvPosFusion();
+
 	void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s& aid_src);
+	void controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s& aid_src);
 
 	// control fusion of optical flow observations
 	void controlOpticalFlowFusion();
@@ -1008,11 +1005,6 @@ private:
 	void startGpsYawFusion(const gpsSample &gps_sample);
 	void stopGpsYawFusion();
 
-	void startEvPosFusion();
-	void startEvYawFusion();
-
-	void stopEvFusion();
-	void stopEvPosFusion();
 	void stopEvVelFusion();
 	void stopEvYawFusion();
 
@@ -1036,11 +1028,13 @@ private:
 	EKFGSF_yaw _yawEstimator{};
 
 	uint8_t _height_sensor_ref{HeightSensor::UNKNOWN};
+	uint8_t _position_sensor_ref{static_cast<uint8_t>(PositionSensor::GNSS)};
 
 	HeightBiasEstimator _baro_b_est{HeightSensor::BARO, _height_sensor_ref};
 	HeightBiasEstimator _gps_hgt_b_est{HeightSensor::GNSS, _height_sensor_ref};
 	HeightBiasEstimator _rng_hgt_b_est{HeightSensor::RANGE, _height_sensor_ref};
 	HeightBiasEstimator _ev_hgt_b_est{HeightSensor::EV, _height_sensor_ref};
+	PositionBiasEstimator _ev_pos_b_est{static_cast<uint8_t>(PositionSensor::EV), _position_sensor_ref};
 
 	void runYawEKFGSF();
 

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -124,22 +124,6 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var)
 	_time_last_ver_vel_fuse = _imu_sample_delayed.time_us;
 }
 
-void Ekf::resetHorizontalPositionToVision()
-{
-	_information_events.flags.reset_pos_to_vision = true;
-	ECL_INFO("reset position to ev position");
-	Vector3f _ev_pos = _ev_sample_delayed.pos;
-
-	if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) {
-		_ev_pos = _R_ev_to_ekf * _ev_sample_delayed.pos;
-	}
-
-	resetHorizontalPositionTo(Vector2f(_ev_pos), _ev_sample_delayed.posVar.slice<2, 1>(0, 0));
-
-	// let the next odometry update know that the previous value of states cannot be used to calculate the change in position
-	_hpos_prev_available = false;
-}
-
 void Ekf::resetHorizontalPositionToLastKnown()
 {
 	_information_events.flags.reset_pos_to_last_known = true;
@@ -178,6 +162,9 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f
 
 	_state_reset_status.reset_count.posNE++;
 
+	_ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - _state_reset_status.posNE_change);
+	//_gps_pos_b_est.setBias(_gps_pos_b_est.getBias() + _state_reset_status.posNE_change);
+
 	// Reset the timout timer
 	_time_last_hor_pos_fuse = _imu_sample_delayed.time_us;
 }
@@ -193,7 +180,6 @@ bool Ekf::isHeightResetRequired() const
 	return (continuous_bad_accel_hgt || hgt_fusion_timeout);
 }
 
-
 void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_var)
 {
 	const float old_vert_pos = _state.pos(2);
@@ -275,8 +261,8 @@ void Ekf::alignOutputFilter()
 bool Ekf::resetMagHeading()
 {
 	// prevent a reset being performed more than once on the same frame
-	if (_imu_sample_delayed.time_us == _flt_mag_align_start_time) {
+	if ((_flt_mag_align_start_time == _imu_sample_delayed.time_us) || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align)) {
-		return true;
+		return false;
 	}
 
 	const Vector3f mag_init = _mag_lpf.getState();
@@ -320,17 +306,6 @@ bool Ekf::resetMagHeading()
 	return false;
 }
 
-bool Ekf::resetYawToEv()
-{
-	const float yaw_new = getEulerYaw(_ev_sample_delayed.quat);
-	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();
-
-	return true;
-}
-
 // Return the magnetic declination in radians to be used by the alignment and fusion processing
 float Ekf::getMagDeclination()
 {
@@ -755,6 +730,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 
 	} else if (_control_status.flags.gps_yaw) {
 		mag = sqrtf(_aid_src_gnss_yaw.test_ratio);
+
 	} else {
 		mag = NAN;
 	}
@@ -1092,7 +1068,7 @@ void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var)
 void Ekf::stopMagFusion()
 {
 	if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
-		ECL_INFO("stopping mag fusion");
+		ECL_INFO("stopping all mag fusion");
 		stopMag3DFusion();
 		stopMagHdgFusion();
 		clearMagCov();
@@ -1166,14 +1142,6 @@ void Ekf::updateGroundEffect()
 	}
 }
 
-// update the rotation matrix which rotates EV measurements into the EKF's navigation frame
-void Ekf::calcExtVisRotMat()
-{
-	// Calculate the quaternion delta that rotates from the EV to the EKF reference frame at the EKF fusion time horizon.
-	const Quatf q_error((_state.quat_nominal * _ev_sample_delayed.quat.inversed()).normalized());
-	_R_ev_to_ekf = Dcmf(q_error);
-}
-
 // Increase the yaw error variance of the quaternions
 // Argument is additional yaw variance in rad**2
 void Ekf::increaseQuatYawErrVariance(float yaw_variance)
@@ -1320,52 +1288,6 @@ void Ekf::stopGpsYawFusion()
 	}
 }
 
-void Ekf::startEvPosFusion()
-{
-	_control_status.flags.ev_pos = true;
-	resetHorizontalPositionToVision();
-	_information_events.flags.starting_vision_pos_fusion = true;
-	ECL_INFO("starting vision pos fusion");
-}
-
-void Ekf::startEvYawFusion()
-{
-	// turn on fusion of external vision yaw measurements and disable all magnetometer fusion
-	_control_status.flags.ev_yaw = true;
-	_control_status.flags.mag_dec = false;
-
-	stopMagHdgFusion();
-	stopMag3DFusion();
-
-	_information_events.flags.starting_vision_yaw_fusion = true;
-	ECL_INFO("starting vision yaw fusion");
-}
-
-void Ekf::stopEvFusion()
-{
-	stopEvPosFusion();
-	stopEvVelFusion();
-	stopEvYawFusion();
-	stopEvHgtFusion();
-}
-
-void Ekf::stopEvPosFusion()
-{
-	if (_control_status.flags.ev_pos) {
-		ECL_INFO("stopping EV pos fusion");
-		_control_status.flags.ev_pos = false;
-		resetEstimatorAidStatus(_aid_src_ev_pos);
-	}
-}
-
-void Ekf::stopEvYawFusion()
-{
-	if (_control_status.flags.ev_yaw) {
-		ECL_INFO("stopping EV yaw fusion");
-		_control_status.flags.ev_yaw = false;
-	}
-}
-
 void Ekf::stopAuxVelFusion()
 {
 	ECL_INFO("stopping aux vel fusion");

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -542,7 +542,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)) || (_params.ev_ctrl > 0)) {
+	if (_params.ev_ctrl > 0) {
 		max_time_delay_ms = math::max(_params.ev_delay_ms, max_time_delay_ms);
 	}
 

src/modules/ekf2/EKF/estimator_interface.h

@@ -258,7 +258,6 @@ public:
 
 	const gpsSample &get_gps_sample_delayed() const { return _gps_sample_delayed; }
 	const rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); }
-	const extVisionSample &get_ev_sample_delayed() const { return _ev_sample_delayed; }
 
 	const bool &global_origin_valid() const { return _NED_origin_initialised; }
 	const MapProjection &global_origin() const { return _pos_ref; }
@@ -305,8 +304,7 @@ protected:
 	sensor::SensorRangeFinder _range_sensor{};
 	airspeedSample _airspeed_sample_delayed{};
 	flowSample _flow_sample_delayed{};
-	extVisionSample _ev_sample_delayed{};
+	extVisionSample _ev_sample_prev{};
-	extVisionSample _ev_sample_delayed_prev{};
 	dragSample _drag_down_sampled{};	// down sampled drag specific force data (filter prediction rate -> observation rate)
 
 	RangeFinderConsistencyCheck _rng_consistency_check;

src/modules/ekf2/EKF/ev_control.cpp

@@ -0,0 +1,86 @@
+/****************************************************************************
+ *
+ *   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_control.cpp
+ * Control functions for ekf external vision control
+ */
+
+#include "ekf.h"
+
+void Ekf::controlExternalVisionFusion()
+{
+	_ev_pos_b_est.predict(_dt_ekf_avg);
+
+	// Check for new external vision data
+	extVisionSample ev_sample;
+
+	if (_ext_vision_buffer && _ext_vision_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &ev_sample)) {
+
+		bool ev_reset = (ev_sample.reset_counter != _ev_sample_prev.reset_counter);
+
+		// determine if we should use the horizontal position observations
+		bool quality_sufficient = (_params.ev_quality_minimum <= 0) || (ev_sample.quality >= _params.ev_quality_minimum);
+
+		const bool starting_conditions_passing = quality_sufficient
+				&& ((ev_sample.time_us - _ev_sample_prev.time_us) < EV_MAX_INTERVAL)
+				&& ((_params.ev_quality_minimum <= 0) || (_ev_sample_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);
+
+		controlEvYawFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_yaw);
+		controlEvVelFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel);
+		controlEvPosFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_pos);
+		controlEvHeightFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_hgt);
+
+		if (quality_sufficient) {
+			_ev_sample_prev = ev_sample;
+		}
+
+		// record corresponding yaw state for future EV delta heading innovation (logging only)
+		_ev_yaw_pred_prev = getEulerYaw(_state.quat_nominal);
+
+	} else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.ev_yaw
+		    || _control_status.flags.ev_hgt)
+		   && isTimedOut(_ev_sample_prev.time_us, 2 * EV_MAX_INTERVAL)) {
+
+		// Turn off EV fusion mode if no data has been received
+		stopEvPosFusion();
+		stopEvVelFusion();
+		stopEvYawFusion();
+		stopEvHgtFusion();
+
+		_warning_events.flags.vision_data_stopped = true;
+		ECL_WARN("vision data stopped");
+	}
+}

src/modules/ekf2/EKF/ev_height_control.cpp

@@ -53,7 +53,7 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
 
 	// rotate measurement into correct earth frame if required
 	Vector3f pos{ev_sample.pos};
-	Matrix3f pos_cov{matrix::diag(ev_sample.posVar)};
+	Matrix3f pos_cov{matrix::diag(ev_sample.position_var)};
 
 	// rotate EV to the EKF reference frame unless we're operating entirely in vision frame
 	// TODO: only necessary if there's a roll/pitch offset between VIO and EKF
@@ -65,7 +65,7 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
 			const Dcmf R_ev_to_ekf(q_error);
 
 			pos = R_ev_to_ekf * ev_sample.pos;
-			pos_cov = R_ev_to_ekf * matrix::diag(ev_sample.posVar) * R_ev_to_ekf.transpose();
+			pos_cov = R_ev_to_ekf * matrix::diag(ev_sample.position_var) * R_ev_to_ekf.transpose();
 
 			// increase minimum variance to include EV orientation variance
 			// TODO: do this properly
@@ -98,9 +98,11 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
 		bias_est.fuseBias(measurement - _state.pos(2), measurement_var + P(9, 9));
 	}
 
-	const bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VPOS)) && measurement_valid;
+	const bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VPOS))
+			&& measurement_valid;
 
-	const bool starting_conditions_passing = common_starting_conditions_passing && continuing_conditions_passing;
+	const bool starting_conditions_passing = common_starting_conditions_passing
+			&& continuing_conditions_passing;
 
 	if (_control_status.flags.ev_hgt) {
 		aid_src.fusion_enabled = true;

src/modules/ekf2/EKF/ev_pos_control.cpp

@@ -0,0 +1,304 @@
+/****************************************************************************
+ *
+ *   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_pos_control.cpp
+ * Control functions for ekf external vision position fusion
+ */
+
+#include "ekf.h"
+
+static constexpr const char *EV_AID_SRC_NAME = "EV position";
+
+
+void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
+			     const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s &aid_src)
+{
+	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 position aiding
+	bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::HPOS))
+					     && _control_status.flags.tilt_align
+					     && PX4_ISFINITE(ev_sample.pos(0))
+					     && PX4_ISFINITE(ev_sample.pos(1));
+
+	// correct position for offset relative to IMU
+	const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
+	const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
+
+	const bool bias_fusion_was_active = _ev_pos_b_est.fusionActive();
+
+	// rotate measurement into correct earth frame if required
+	Vector3f pos{NAN, NAN, NAN};
+	Matrix3f pos_cov{};
+
+	switch (ev_sample.pos_frame) {
+	case PositionFrame::LOCAL_FRAME_NED:
+		if (_control_status.flags.yaw_align) {
+			pos = ev_sample.pos - pos_offset_earth;
+			pos_cov = matrix::diag(ev_sample.position_var);
+
+			if (_control_status.flags.gps) {
+				_ev_pos_b_est.setFusionActive();
+
+			} else {
+				_ev_pos_b_est.setFusionInactive();
+			}
+
+		} else {
+			continuing_conditions_passing = false;
+			_ev_pos_b_est.setFusionInactive();
+			_ev_pos_b_est.reset();
+		}
+
+		break;
+
+	case PositionFrame::LOCAL_FRAME_FRD:
+		if (_control_status.flags.ev_yaw) {
+			// using EV frame
+			pos = ev_sample.pos - pos_offset_earth;
+			pos_cov = matrix::diag(ev_sample.position_var);
+
+			_ev_pos_b_est.setFusionInactive();
+			_ev_pos_b_est.reset();
+
+		} 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);
+
+			pos = R_ev_to_ekf * ev_sample.pos - pos_offset_earth;
+			pos_cov = R_ev_to_ekf * matrix::diag(ev_sample.position_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++) {
+				pos_cov(i, i) = math::max(pos_cov(i, i), orientation_var_max);
+			}
+
+			if (_control_status.flags.gps) {
+				_ev_pos_b_est.setFusionActive();
+
+			} else {
+				_ev_pos_b_est.setFusionInactive();
+			}
+		}
+
+		break;
+
+	default:
+		continuing_conditions_passing = false;
+		_ev_pos_b_est.setFusionInactive();
+		_ev_pos_b_est.reset();
+		break;
+	}
+
+	// increase minimum variance if GPS active (position reference)
+	if (_control_status.flags.gps) {
+		for (int i = 0; i < 2; i++) {
+			pos_cov(i, i) = math::max(pos_cov(i, i), sq(_params.gps_pos_noise));
+		}
+	}
+
+	const Vector2f measurement{pos(0), pos(1)};
+
+	const Vector2f measurement_var{
+		math::max(pos_cov(0, 0), sq(_params.ev_pos_noise), sq(0.01f)),
+		math::max(pos_cov(1, 1), sq(_params.ev_pos_noise), sq(0.01f))
+	};
+
+	const bool measurement_valid = measurement.isAllFinite() && measurement_var.isAllFinite();
+
+	// bias fusion activated (GPS activated)
+	if (!bias_fusion_was_active && _ev_pos_b_est.fusionActive()) {
+		if (quality_sufficient) {
+			// reset the bias estimator
+			_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
+
+		} else if (isOtherSourceOfHorizontalAidingThan(_control_status.flags.ev_pos)) {
+			// otherwise stop EV position, when quality is good again it will restart with reset bias
+			stopEvPosFusion();
+		}
+	}
+
+	updateHorizontalPositionAidSrcStatus(ev_sample.time_us,
+					     measurement - _ev_pos_b_est.getBias(),        // observation
+					     measurement_var + _ev_pos_b_est.getBiasVar(), // observation variance
+					     math::max(_params.ev_pos_innov_gate, 1.f),    // innovation gate
+					     aid_src);
+
+	// update the bias estimator before updating the main filter but after
+	// using its current state to compute the vertical position innovation
+	if (measurement_valid && quality_sufficient) {
+		_ev_pos_b_est.setMaxStateNoise(Vector2f(sqrtf(measurement_var(0)), sqrtf(measurement_var(1))));
+		_ev_pos_b_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd); // TODO
+		_ev_pos_b_est.fuseBias(measurement - Vector2f(_state.pos.xy()), measurement_var + Vector2f(P(7, 7), P(8, 8)));
+	}
+
+	if (!measurement_valid) {
+		continuing_conditions_passing = false;
+	}
+
+	const bool starting_conditions_passing = common_starting_conditions_passing
+			&& continuing_conditions_passing;
+
+	if (_control_status.flags.ev_pos) {
+		aid_src.fusion_enabled = true;
+
+		if (continuing_conditions_passing) {
+			const bool bias_estimator_change = (bias_fusion_was_active != _ev_pos_b_est.fusionActive());
+			const bool reset = ev_reset || yaw_alignment_changed || bias_estimator_change;
+
+			updateEvPosFusion(measurement, measurement_var, quality_sufficient, reset, aid_src);
+
+		} else {
+			// Stop fusion but do not declare it faulty
+			ECL_WARN("stopping %s fusion, continuing conditions failing", EV_AID_SRC_NAME);
+			stopEvPosFusion();
+		}
+
+	} else {
+		if (starting_conditions_passing) {
+			startEvPosFusion(measurement, measurement_var, aid_src);
+		}
+	}
+}
+
+void Ekf::startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, estimator_aid_source2d_s &aid_src)
+{
+	// activate fusion
+	// TODO:  (_params.position_sensor_ref == PositionSensor::EV)
+	if (_control_status.flags.gps) {
+		ECL_INFO("starting %s fusion", EV_AID_SRC_NAME);
+		_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
+		_ev_pos_b_est.setFusionActive();
+
+	} else {
+		ECL_INFO("starting %s fusion, resetting state", EV_AID_SRC_NAME);
+		//_position_sensor_ref = PositionSensor::EV;
+		_information_events.flags.reset_pos_to_vision = true;
+		resetHorizontalPositionTo(measurement, measurement_var);
+		_ev_pos_b_est.reset();
+	}
+
+	aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+
+	_nb_ev_pos_reset_available = 5;
+	_information_events.flags.starting_vision_pos_fusion = true;
+	_control_status.flags.ev_pos = true;
+}
+
+void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, bool reset, estimator_aid_source2d_s &aid_src)
+{
+	if (reset) {
+
+		if (quality_sufficient) {
+
+			if (!_control_status.flags.gps) {
+				ECL_INFO("reset to %s", EV_AID_SRC_NAME);
+				_information_events.flags.reset_pos_to_vision = true;
+				resetHorizontalPositionTo(measurement, measurement_var);
+				_ev_pos_b_est.reset();
+
+			} else {
+				_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
+			}
+
+			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
+			stopEvPosFusion();
+			return;
+		}
+
+	} else if (quality_sufficient) {
+		fuseHorizontalPosition(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) {
+		bool pos_xy_fusion_failing = isTimedOut(_time_last_hor_pos_fuse, _params.no_aid_timeout_max);
+
+		if ((_nb_ev_pos_reset_available > 0) && quality_sufficient) {
+			// Data seems good, attempt a reset
+			ECL_WARN("%s fusion failing, resetting", EV_AID_SRC_NAME);
+
+			if (_control_status.flags.gps && !pos_xy_fusion_failing) {
+				// reset EV position bias
+				_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
+
+			} else {
+				_information_events.flags.reset_pos_to_vision = true;
+
+				if (_control_status.flags.gps) {
+					resetHorizontalPositionTo(measurement - _ev_pos_b_est.getBias(), measurement_var + _ev_pos_b_est.getBiasVar());
+					_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
+
+				} else {
+					resetHorizontalPositionTo(measurement, measurement_var);
+					_ev_pos_b_est.reset();
+				}
+			}
+
+			aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+
+			if (_control_status.flags.in_air) {
+				_nb_ev_pos_reset_available--;
+			}
+
+		} 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", EV_AID_SRC_NAME);
+			stopEvPosFusion();
+		}
+	}
+}
+
+void Ekf::stopEvPosFusion()
+{
+	if (_control_status.flags.ev_pos) {
+		resetEstimatorAidStatus(_aid_src_ev_pos);
+
+		_control_status.flags.ev_pos = false;
+	}
+}

src/modules/ekf2/EKF/ev_vel_control.cpp

@@ -134,7 +134,8 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 		continuing_conditions_passing = false;
 	}
 
-	const bool starting_conditions_passing = common_starting_conditions_passing && continuing_conditions_passing
+	const bool starting_conditions_passing = common_starting_conditions_passing
+			&& continuing_conditions_passing
 			&& ((Vector3f(aid_src.test_ratio).max() < 0.1f) || !isHorizontalAidingActive());
 
 	if (_control_status.flags.ev_vel) {

src/modules/ekf2/EKF/ev_yaw_control.cpp

@@ -0,0 +1,186 @@
+/****************************************************************************
+ *
+ *   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_yaw_control.cpp
+ * Control functions for ekf external vision yaw fusion
+ */
+
+#include "ekf.h"
+
+void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
+			     const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src)
+{
+	static constexpr const char *AID_SRC_NAME = "EV yaw";
+
+	resetEstimatorAidStatus(aid_src);
+	aid_src.timestamp_sample = ev_sample.time_us;
+	aid_src.observation = getEulerYaw(ev_sample.quat);
+	aid_src.observation_variance = math::max(ev_sample.orientation_var(2), _params.ev_att_noise, sq(0.01f));
+	aid_src.innovation = wrap_pi(getEulerYaw(_R_to_earth) - aid_src.observation);
+
+	// determine if we should use EV yaw aiding
+	bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::YAW))
+					     && _control_status.flags.tilt_align
+					     && !_inhibit_ev_yaw_use
+					     && PX4_ISFINITE(aid_src.observation)
+					     && PX4_ISFINITE(aid_src.observation_variance);
+
+	// if GPS enabled don't allow EV yaw if EV isn't NED
+	if (_control_status.flags.gps && _control_status.flags.yaw_align
+	    && (ev_sample.pos_frame != PositionFrame::LOCAL_FRAME_NED)
+	   ) {
+		continuing_conditions_passing = false;
+
+		// use delta yaw for innovation logging
+		aid_src.innovation = wrap_pi(wrap_pi(getEulerYaw(_R_to_earth) - _ev_yaw_pred_prev)
+					     - wrap_pi(getEulerYaw(ev_sample.quat) - getEulerYaw(_ev_sample_prev.quat)));
+	}
+
+	const bool starting_conditions_passing = common_starting_conditions_passing
+			&& continuing_conditions_passing
+			&& isTimedOut(aid_src.time_last_fuse, (uint32_t)1e6);
+
+	if (_control_status.flags.ev_yaw) {
+		aid_src.fusion_enabled = true;
+
+		if (continuing_conditions_passing) {
+
+			if (ev_reset) {
+
+				if (quality_sufficient) {
+					ECL_INFO("reset to %s", AID_SRC_NAME);
+					//_information_events.flags.reset_yaw_to_vision = true; // TODO
+					resetQuatStateYaw(aid_src.observation, aid_src.observation_variance);
+					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
+					stopEvYawFusion();
+					return;
+				}
+
+			} else if (quality_sufficient) {
+				fuseYaw(aid_src.innovation, aid_src.observation_variance, aid_src);
+
+			} else {
+				aid_src.innovation_rejected = true;
+			}
+
+			const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.no_aid_timeout_max);
+
+			if (is_fusion_failing) {
+				if ((_nb_ev_yaw_reset_available > 0) && quality_sufficient) {
+					// Data seems good, attempt a reset
+					//_information_events.flags.reset_yaw_to_vision = true; // TODO
+					ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME);
+					resetQuatStateYaw(aid_src.innovation, aid_src.observation_variance);
+					aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+
+					if (_control_status.flags.in_air) {
+						_nb_ev_yaw_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_yaw_fault = true;
+					ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME);
+					stopEvYawFusion();
+
+				} 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);
+					stopEvYawFusion();
+				}
+			}
+
+		} else {
+			// Stop fusion but do not declare it faulty
+			ECL_WARN("stopping %s fusion, continuing conditions failing", AID_SRC_NAME);
+			stopEvYawFusion();
+		}
+
+	} else {
+		if (starting_conditions_passing) {
+			// activate fusion
+			if (ev_sample.pos_frame == PositionFrame::LOCAL_FRAME_NED) {
+
+				if (_control_status.flags.yaw_align) {
+					ECL_INFO("starting %s fusion", AID_SRC_NAME);
+
+				} else {
+					// reset yaw to EV and set yaw_align
+					ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME);
+					resetQuatStateYaw(aid_src.observation, aid_src.observation_variance);
+					_control_status.flags.yaw_align = true;
+				}
+
+				aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+				_information_events.flags.starting_vision_yaw_fusion = true;
+				_control_status.flags.ev_yaw = true;
+
+			} else if (ev_sample.pos_frame == PositionFrame::LOCAL_FRAME_FRD) {
+				// turn on fusion of external vision yaw measurements and disable all other heading fusion
+				stopMagFusion();
+				stopGpsYawFusion();
+				stopGpsFusion();
+
+				ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME);
+
+				// reset yaw to EV
+				resetQuatStateYaw(aid_src.observation, aid_src.observation_variance);
+				aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+
+				_information_events.flags.starting_vision_yaw_fusion = true;
+				_control_status.flags.yaw_align = false;
+				_control_status.flags.ev_yaw = true;
+			}
+
+			if (_control_status.flags.ev_yaw) {
+				_nb_ev_yaw_reset_available = 5;
+			}
+		}
+	}
+}
+
+void Ekf::stopEvYawFusion()
+{
+	if (_control_status.flags.ev_yaw) {
+		resetEstimatorAidStatus(_aid_src_ev_yaw);
+
+		_control_status.flags.ev_yaw = false;
+	}
+}

src/modules/ekf2/EKF/fake_pos_control.cpp

@@ -95,7 +95,6 @@ void Ekf::controlFakePosFusion()
 			if (starting_conditions_passing) {
 				ECL_INFO("start fake position fusion");
 				_control_status.flags.fake_pos = true;
-				_fuse_hpos_as_odom = false; // TODO: needed?
 				resetFakePosFusion();
 
 				if (_control_status.flags.tilt_align) {

src/modules/ekf2/EKF/gps_control.cpp

@@ -57,16 +57,18 @@ void Ekf::controlGpsFusion()
 		controlGpsYawFusion(gps_sample, gps_checks_passing, gps_checks_failing);
 
 		// GNSS velocity
+		const Vector3f velocity{gps_sample.vel};
 		const float vel_var = sq(math::max(gps_sample.sacc, _params.gps_vel_noise));
 		const Vector3f vel_obs_var(vel_var, vel_var, vel_var * sq(1.5f));
 		updateVelocityAidSrcStatus(gps_sample.time_us,
-					   gps_sample.vel,                                             // observation
+					   velocity,                                                   // observation
 					   vel_obs_var,                                                // observation variance
 					   math::max(_params.gps_vel_innov_gate, 1.f),                 // innovation gate
 					   _aid_src_gnss_vel);
 		_aid_src_gnss_vel.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::VEL);
 
 		// GNSS position
+		const Vector2f position{gps_sample.pos};
 		// relax the upper observation noise limit which prevents bad GPS perturbing the position estimate
 		float pos_noise = math::max(gps_sample.hacc, _params.gps_pos_noise);
 
@@ -81,12 +83,34 @@ void Ekf::controlGpsFusion()
 		const float pos_var = sq(pos_noise);
 		const Vector2f pos_obs_var(pos_var, pos_var);
 		updateHorizontalPositionAidSrcStatus(gps_sample.time_us,
-						     gps_sample.pos,                             // observation
+						     position,                                   // observation
 						     pos_obs_var,                                // observation variance
 						     math::max(_params.gps_pos_innov_gate, 1.f), // innovation gate
 						     _aid_src_gnss_pos);
 		_aid_src_gnss_pos.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::HPOS);
 
+		// if GPS is otherwise ready to go, but yaw_align is blocked by EV give mag a chance to start
+		if (_control_status.flags.tilt_align && _NED_origin_initialised
+		    && gps_checks_passing && !gps_checks_failing) {
+
+			if (!_control_status.flags.yaw_align) {
+				if (_control_status.flags.ev_yaw && !_control_status.flags.yaw_align) {
+
+					// give mag a chance to start and yaw align if currently blocked by EV yaw
+					const bool mag_enabled = (_params.mag_fusion_type <= MagFuseType::MAG_3D);
+					const bool mag_available = (_mag_counter != 0);
+
+					if (mag_enabled && mag_available
+					    && !_control_status.flags.mag_field_disturbed
+					    && !_control_status.flags.mag_fault) {
+
+						stopEvYawFusion();
+					}
+				}
+			}
+		}
+
+
 		// Determine if we should use GPS aiding for velocity and horizontal position
 		// To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently
 		const bool mandatory_conditions_passing = ((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VEL))
@@ -134,12 +158,6 @@ void Ekf::controlGpsFusion()
 					stopGpsFusion();
 					_warning_events.flags.gps_quality_poor = true;
 					ECL_WARN("GPS quality poor - stopping use");
-
-					// TODO: move this to EV control logic
-					// Reset position state to external vision if we are going to use absolute values
-					if (_control_status.flags.ev_pos && !(_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS)) {
-						resetHorizontalPositionToVision();
-					}
 				}
 
 			} else { // mandatory conditions are not passing
@@ -150,44 +168,48 @@ void Ekf::controlGpsFusion()
 			if (starting_conditions_passing) {
 				// Do not use external vision for yaw if using GPS because yaw needs to be
 				// defined relative to an NED reference frame
-				if (_control_status.flags.ev_yaw
+				if (_control_status.flags.ev_yaw) {
-				    || _mag_inhibit_yaw_reset_req
-				    || _mag_yaw_reset_req) {
-
-					_mag_yaw_reset_req = true;
-
 					// Stop the vision for yaw fusion and do not allow it to start again
 					stopEvYawFusion();
 					_inhibit_ev_yaw_use = true;
-
-				} else {
-					ECL_INFO("starting GPS fusion");
-					_information_events.flags.starting_gps_fusion = true;
-
-					// reset position
-					_information_events.flags.reset_pos_to_gps = true;
-					resetHorizontalPositionTo(gps_sample.pos, pos_obs_var);
-
-					// when already using another velocity source velocity reset is not necessary
-					if (!isHorizontalAidingActive()) {
-						_information_events.flags.reset_vel_to_gps = true;
-						resetVelocityTo(gps_sample.vel, vel_obs_var);
-					}
-
-					_control_status.flags.gps = true;
 				}
 
+				ECL_INFO("starting GPS fusion");
+				_information_events.flags.starting_gps_fusion = true;
+
+				// when already using another velocity source velocity reset is not necessary
+				if (!isHorizontalAidingActive()
+				    || isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
+				    || !_control_status_prev.flags.yaw_align
+				   ) {
+					// reset velocity
+					_information_events.flags.reset_vel_to_gps = true;
+					resetVelocityTo(velocity, vel_obs_var);
+					_aid_src_gnss_vel.time_last_fuse = _imu_sample_delayed.time_us;
+				}
+
+				// reset position
+				_information_events.flags.reset_pos_to_gps = true;
+				resetHorizontalPositionTo(position, pos_obs_var);
+				_aid_src_gnss_pos.time_last_fuse = _imu_sample_delayed.time_us;
+
+				_control_status.flags.gps = true;
+
 			} else if (gps_checks_passing && !_control_status.flags.yaw_align && (_params.mag_fusion_type == MagFuseType::NONE)) {
 				// If no mag is used, align using the yaw estimator (if available)
 				if (resetYawToEKFGSF()) {
 					_information_events.flags.yaw_aligned_to_imu_gps = true;
-					ECL_INFO("Yaw aligned using IMU and GPS");
+					ECL_INFO("GPS yaw aligned using IMU, resetting vel and pos");
 
-					ECL_INFO("reset velocity and position to GPS");
+					// reset velocity
 					_information_events.flags.reset_vel_to_gps = true;
+					resetVelocityTo(velocity, vel_obs_var);
+					_aid_src_gnss_vel.time_last_fuse = _imu_sample_delayed.time_us;
+
+					// reset position
 					_information_events.flags.reset_pos_to_gps = true;
-					resetVelocityTo(gps_sample.vel, vel_obs_var);
+					resetHorizontalPositionTo(position, pos_obs_var);
-					resetHorizontalPositionTo(gps_sample.pos, pos_obs_var);
+					_aid_src_gnss_pos.time_last_fuse = _imu_sample_delayed.time_us;
 				}
 			}
 		}

src/modules/ekf2/EKF/mag_control.cpp

@@ -218,6 +218,12 @@ bool Ekf::canResetMagHeading() const
 
 void Ekf::runInAirYawReset()
 {
+	// prevent a reset being performed more than once on the same frame
+	if ((_flt_mag_align_start_time == _imu_sample_delayed.time_us)
+	    || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align)) {
+		return;
+	}
+
 	if (_mag_yaw_reset_req && !_is_yaw_fusion_inhibited) {
 		bool has_realigned_yaw = false;
 
@@ -252,6 +258,8 @@ void Ekf::runInAirYawReset()
 				);
 
 			resetMagCov();
+
+			has_realigned_yaw = true;
 		}
 
 		if (!has_realigned_yaw && canResetMagHeading()) {
@@ -296,14 +304,19 @@ void Ekf::check3DMagFusionSuitability()
 
 void Ekf::checkYawAngleObservability()
 {
-	// Check if there has been enough change in horizontal velocity to make yaw observable
+	if (_control_status.flags.gps) {
-	// Apply hysteresis to check to avoid rapid toggling
+		// Check if there has been enough change in horizontal velocity to make yaw observable
-	_yaw_angle_observable = _yaw_angle_observable
+		// Apply hysteresis to check to avoid rapid toggling
-				? _accel_lpf_NE.norm() > _params.mag_acc_gate
+		if (_yaw_angle_observable) {
-				: _accel_lpf_NE.norm() > 2.0f * _params.mag_acc_gate;
+			_yaw_angle_observable = _accel_lpf_NE.norm() > _params.mag_acc_gate;
 
-	_yaw_angle_observable = _yaw_angle_observable
+		} else {
-				&& (_control_status.flags.gps || _control_status.flags.ev_pos); // Do we have to add ev_vel here?
+			_yaw_angle_observable = _accel_lpf_NE.norm() > _params.mag_acc_gate * 2.f;
+		}
+
+	} else {
+		_yaw_angle_observable = false;
+	}
 }
 
 void Ekf::checkMagBiasObservability()
@@ -366,9 +379,7 @@ bool Ekf::shouldInhibitMag() const
 	// has explicitly stopped magnetometer use.
 	const bool user_selected = (_params.mag_fusion_type == MagFuseType::INDOOR);
 
-	const bool heading_not_required_for_navigation = !_control_status.flags.gps
+	const bool heading_not_required_for_navigation = !_control_status.flags.gps;
-			&& !_control_status.flags.ev_pos
-			&& !_control_status.flags.ev_vel;
 
 	return (user_selected && heading_not_required_for_navigation) || _control_status.flags.mag_field_disturbed;
 }

src/modules/ekf2/EKF/position_bias_estimator.hpp

@@ -0,0 +1,113 @@
+/****************************************************************************
+ *
+ *   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 position_bias_estimator.hpp
+ */
+
+#pragma once
+
+#include "bias_estimator.hpp"
+
+class PositionBiasEstimator
+{
+public:
+	PositionBiasEstimator(uint8_t sensor, const uint8_t &sensor_ref):
+		_sensor(sensor),
+		_sensor_ref(sensor_ref)
+	{}
+	virtual ~PositionBiasEstimator() = default;
+
+	bool fusionActive() const { return _is_sensor_fusion_active; }
+
+	void setFusionActive() { _is_sensor_fusion_active = true; }
+	void setFusionInactive() { _is_sensor_fusion_active = false; }
+
+	void predict(float dt)
+	{
+		if ((_sensor_ref != _sensor) && _is_sensor_fusion_active) {
+			_bias[0].predict(dt);
+			_bias[1].predict(dt);
+		}
+	}
+
+	void fuseBias(Vector2f bias, Vector2f bias_var)
+	{
+		if ((_sensor_ref != _sensor) && _is_sensor_fusion_active) {
+			_bias[0].fuseBias(bias(0), bias_var(0));
+			_bias[1].fuseBias(bias(1), bias_var(1));
+		}
+	}
+
+	void setBias(const Vector2f &bias)
+	{
+		_bias[0].setBias(bias(0));
+		_bias[1].setBias(bias(1));
+	}
+
+	void setProcessNoiseSpectralDensity(float nsd)
+	{
+		_bias[0].setProcessNoiseSpectralDensity(nsd);
+		_bias[1].setProcessNoiseSpectralDensity(nsd);
+	}
+	// void setBiasStdDev(float state_noise) { _state_var = state_noise * state_noise; }
+	// void setInnovGate(float gate_size) { _gate_size = gate_size; }
+
+	void setMaxStateNoise(const Vector2f &max_noise)
+	{
+		_bias[0].setMaxStateNoise(max_noise(0));
+		_bias[1].setMaxStateNoise(max_noise(1));
+	}
+
+	Vector2f getBias() const { return Vector2f(_bias[0].getBias(), _bias[1].getBias()); }
+	float getBias(int index) const { return _bias[index].getBias(); }
+
+	Vector2f getBiasVar() const { return Vector2f(_bias[0].getBiasVar(), _bias[1].getBiasVar()); }
+	float getBiasVar(int index) const { return _bias[index].getBiasVar(); }
+
+	const BiasEstimator::status &getStatus(int index) const { return _bias[index].getStatus(); }
+
+	void reset()
+	{
+		_bias[0].reset();
+		_bias[1].reset();
+	}
+
+private:
+	BiasEstimator _bias[2] {};
+
+	const uint8_t _sensor;
+	const uint8_t &_sensor_ref;
+
+	bool _is_sensor_fusion_active{false}; // TODO: replace by const ref and remove setter when migrating _control_status.flags from union to bool
+};

src/modules/ekf2/EKF2.cpp

@@ -207,23 +207,67 @@ bool EKF2::multi_init(int imu, int mag)
 {
 	// advertise all topics to ensure consistent uORB instance numbering
 	_ekf2_timestamps_pub.advertise();
-	_estimator_baro_bias_pub.advertise();
-	_estimator_ev_hgt_bias_pub.advertise();
 	_estimator_event_flags_pub.advertise();
-	_estimator_gnss_hgt_bias_pub.advertise();
-	_estimator_gps_status_pub.advertise();
 	_estimator_innovation_test_ratios_pub.advertise();
 	_estimator_innovation_variances_pub.advertise();
 	_estimator_innovations_pub.advertise();
 	_estimator_optical_flow_vel_pub.advertise();
-	_estimator_rng_hgt_bias_pub.advertise();
 	_estimator_sensor_bias_pub.advertise();
 	_estimator_states_pub.advertise();
 	_estimator_status_flags_pub.advertise();
 	_estimator_status_pub.advertise();
-	_estimator_visual_odometry_aligned_pub.advertise();
 	_yaw_est_pub.advertise();
 
+	// baro advertise
+	if (_param_ekf2_baro_ctrl.get()) {
+		_estimator_aid_src_baro_hgt_pub.advertise();
+		_estimator_baro_bias_pub.advertise();
+	}
+
+	// EV advertise
+	if (_param_ekf2_ev_ctrl.get() & static_cast<int32_t>(EvCtrl::VPOS)) {
+		_estimator_aid_src_ev_hgt_pub.advertise();
+		_estimator_ev_pos_bias_pub.advertise();
+	}
+
+	if (_param_ekf2_ev_ctrl.get() & static_cast<int32_t>(EvCtrl::HPOS)) {
+		_estimator_aid_src_ev_pos_pub.advertise();
+		_estimator_ev_pos_bias_pub.advertise();
+	}
+
+	if (_param_ekf2_ev_ctrl.get() & static_cast<int32_t>(EvCtrl::VEL)) {
+		_estimator_aid_src_ev_vel_pub.advertise();
+	}
+
+	if (_param_ekf2_ev_ctrl.get() & static_cast<int32_t>(EvCtrl::YAW)) {
+		_estimator_aid_src_ev_yaw_pub.advertise();
+	}
+
+	// GNSS advertise
+	if (_param_ekf2_gps_ctrl.get() & static_cast<int32_t>(GnssCtrl::VPOS)) {
+		_estimator_aid_src_gnss_hgt_pub.advertise();
+		_estimator_gnss_hgt_bias_pub.advertise();
+	}
+
+	if (_param_ekf2_gps_ctrl.get() & static_cast<int32_t>(GnssCtrl::HPOS)) {
+		_estimator_aid_src_gnss_pos_pub.advertise();
+		_estimator_gps_status_pub.advertise();
+	}
+
+	if (_param_ekf2_gps_ctrl.get() & static_cast<int32_t>(GnssCtrl::VEL)) {
+		_estimator_aid_src_gnss_vel_pub.advertise();
+	}
+
+	if (_param_ekf2_gps_ctrl.get() & static_cast<int32_t>(GnssCtrl::YAW)) {
+		_estimator_aid_src_gnss_yaw_pub.advertise();
+	}
+
+	// RNG advertise
+	if (_param_ekf2_rng_ctrl.get()) {
+		_estimator_aid_src_rng_hgt_pub.advertise();
+		_estimator_rng_hgt_bias_pub.advertise();
+	}
+
 	_attitude_pub.advertise();
 	_local_position_pub.advertise();
 	_global_position_pub.advertise();
@@ -545,15 +589,13 @@ void EKF2::Run()
 		UpdateAirspeedSample(ekf2_timestamps);
 		UpdateAuxVelSample(ekf2_timestamps);
 		UpdateBaroSample(ekf2_timestamps);
+		UpdateExtVisionSample(ekf2_timestamps);
 		UpdateFlowSample(ekf2_timestamps);
 		UpdateGpsSample(ekf2_timestamps);
 		UpdateMagSample(ekf2_timestamps);
 		UpdateRangeSample(ekf2_timestamps);
 		UpdateSystemFlagsSample(ekf2_timestamps);
 
-		vehicle_odometry_s ev_odom;
-		const bool new_ev_odom = UpdateExtVisionSample(ekf2_timestamps, ev_odom);
-
 		// run the EKF update and output
 		const hrt_abstime ekf_update_start = hrt_absolute_time();
 
@@ -569,7 +611,7 @@ void EKF2::Run()
 			PublishBaroBias(now);
 			PublishGnssHgtBias(now);
 			PublishRngHgtBias(now);
-			PublishEvHgtBias(now);
+			PublishEvPosBias(now);
 			PublishEventFlags(now);
 			PublishGpsStatus(now);
 			PublishInnovations(now);
@@ -593,11 +635,6 @@ void EKF2::Run()
 			perf_set_elapsed(_ecl_ekf_update_perf, hrt_elapsed_time(&ekf_update_start));
 		}
 
-		// publish external visual odometry after fixed frame alignment if new odometry is received
-		if (new_ev_odom) {
-			PublishOdometryAligned(now, ev_odom);
-		}
-
 		// publish ekf2_timestamps
 		_ekf2_timestamps_pub.publish(ekf2_timestamps);
 	}
@@ -666,12 +703,32 @@ void EKF2::VerifyParams()
 	}
 
 	// EV EKF2_AID_MASK -> EKF2_EV_CTRL
-	if ((_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_EXT_VIS_VEL)) {
+	if ((_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_EXT_VIS_VEL)
+	    || (_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_EXT_VIS_POS)
+	    || (_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_EXT_VIS_YAW)
+	   ) {
 
-		_param_ekf2_ev_ctrl.set(_param_ekf2_ev_ctrl.get() | static_cast<int32_t>(EvCtrl::VEL));
+		// EKF2_EV_CTRL set VEL bit
-		_param_ekf2_ev_ctrl.commit();
+		if ((_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_EXT_VIS_VEL)) {
+			_param_ekf2_ev_ctrl.set(_param_ekf2_ev_ctrl.get() | static_cast<int32_t>(EvCtrl::VEL));
+		}
+
+		// EKF2_EV_CTRL set HPOS/VPOS bits
+		if ((_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_EXT_VIS_POS)) {
+			_param_ekf2_ev_ctrl.set(_param_ekf2_ev_ctrl.get()
+						| static_cast<int32_t>(EvCtrl::HPOS) | static_cast<int32_t>(EvCtrl::VPOS));
+		}
+
+		// EKF2_EV_CTRL set YAW bit
+		if ((_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_EXT_VIS_YAW)) {
+			_param_ekf2_ev_ctrl.set(_param_ekf2_ev_ctrl.get() | static_cast<int32_t>(EvCtrl::YAW));
+		}
 
 		_param_ekf2_aid_mask.set(_param_ekf2_aid_mask.get() & ~(SensorFusionMask::DEPRECATED_USE_EXT_VIS_VEL));
+		_param_ekf2_aid_mask.set(_param_ekf2_aid_mask.get() & ~(SensorFusionMask::DEPRECATED_USE_EXT_VIS_POS));
+		_param_ekf2_aid_mask.set(_param_ekf2_aid_mask.get() & ~(SensorFusionMask::DEPRECATED_USE_EXT_VIS_YAW));
+
+		_param_ekf2_ev_ctrl.commit();
 		_param_ekf2_aid_mask.commit();
 
 		mavlink_log_critical(&_mavlink_log_pub, "EKF2 EV use EKF2_EV_CTRL instead of EKF2_AID_MASK\n");
@@ -787,15 +844,35 @@ void EKF2::PublishRngHgtBias(const hrt_abstime &timestamp)
 	}
 }
 
-void EKF2::PublishEvHgtBias(const hrt_abstime &timestamp)
+void EKF2::PublishEvPosBias(const hrt_abstime &timestamp)
 {
-	if (_ekf.get_ev_sample_delayed().time_us != 0) {
+	if (_ekf.aid_src_ev_hgt().timestamp_sample) {
-		const BiasEstimator::status &status = _ekf.getEvHgtBiasEstimatorStatus();
 
-		if (fabsf(status.bias - _last_ev_hgt_bias_published) > 0.001f) {
+		estimator_bias3d_s bias{};
-			_estimator_ev_hgt_bias_pub.publish(fillEstimatorBiasMsg(status, _ekf.get_ev_sample_delayed().time_us, timestamp));
 
-			_last_ev_hgt_bias_published = status.bias;
+		// height
+		BiasEstimator::status bias_est_status[3];
+		bias_est_status[0] = _ekf.getEvPosBiasEstimatorStatus(0);
+		bias_est_status[1] = _ekf.getEvPosBiasEstimatorStatus(1);
+		bias_est_status[2] = _ekf.getEvHgtBiasEstimatorStatus();
+
+		for (int i = 0; i < 3; i++) {
+			bias.bias[i] = bias_est_status[i].bias;
+			bias.bias_var[i] = bias_est_status[i].bias_var;
+
+			bias.innov[i] = bias_est_status[i].innov;
+			bias.innov_var[i] = bias_est_status[i].innov_var;
+			bias.innov_test_ratio[i] = bias_est_status[i].innov_test_ratio;
+		}
+
+		const Vector3f bias_vec{bias.bias};
+
+		if ((bias_vec - _last_ev_bias_published).longerThan(0.01f)) {
+			bias.timestamp_sample = _ekf.aid_src_ev_hgt().timestamp_sample;
+			bias.timestamp = hrt_absolute_time();
+			_estimator_ev_pos_bias_pub.publish(bias);
+
+			_last_ev_bias_published = Vector3f(bias.bias);
 		}
 	}
 }
@@ -1210,43 +1287,6 @@ void EKF2::PublishOdometry(const hrt_abstime &timestamp)
 	_odometry_pub.publish(odom);
 }
 
-void EKF2::PublishOdometryAligned(const hrt_abstime &timestamp, const vehicle_odometry_s &ev_odom)
-{
-	const Quatf quat_ev2ekf = _ekf.getVisionAlignmentQuaternion(); // rotates from EV to EKF navigation frame
-	const Dcmf ev_rot_mat(quat_ev2ekf);
-
-	vehicle_odometry_s aligned_ev_odom{ev_odom};
-
-	// Rotate external position and velocity into EKF navigation frame
-	const Vector3f aligned_pos = ev_rot_mat * Vector3f(ev_odom.position);
-	aligned_pos.copyTo(aligned_ev_odom.position);
-
-	switch (ev_odom.velocity_frame) {
-	case vehicle_odometry_s::VELOCITY_FRAME_BODY_FRD: {
-			const Vector3f aligned_vel = Dcmf(_ekf.getQuaternion()) * Vector3f(ev_odom.velocity);
-			aligned_vel.copyTo(aligned_ev_odom.velocity);
-			break;
-		}
-
-	case vehicle_odometry_s::VELOCITY_FRAME_FRD: {
-			const Vector3f aligned_vel = ev_rot_mat * Vector3f(ev_odom.velocity);
-			aligned_vel.copyTo(aligned_ev_odom.velocity);
-			break;
-		}
-	}
-
-	aligned_ev_odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_NED;
-
-	// Compute orientation in EKF navigation frame
-	Quatf ev_quat_aligned = quat_ev2ekf * Quatf(ev_odom.q);
-	ev_quat_aligned.normalize();
-
-	ev_quat_aligned.copyTo(aligned_ev_odom.q);
-
-	aligned_ev_odom.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
-	_estimator_visual_odometry_aligned_pub.publish(aligned_ev_odom);
-}
-
 void EKF2::PublishSensorBias(const hrt_abstime &timestamp)
 {
 	// estimator_sensor_bias
@@ -1700,12 +1740,14 @@ void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps)
 	}
 }
 
-bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odometry_s &ev_odom)
+bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps)
 {
 	// EKF external vision sample
 	bool new_ev_odom = false;
 	const unsigned last_generation = _ev_odom_sub.get_last_generation();
 
+	vehicle_odometry_s ev_odom;
+
 	if (_ev_odom_sub.update(&ev_odom)) {
 		if (_msg_missed_odometry_perf == nullptr) {
 			_msg_missed_odometry_perf = perf_alloc(PC_COUNT, MODULE_NAME": vehicle_visual_odometry messages missed");
@@ -1764,38 +1806,38 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo
 		}
 
 		// check for valid position data
-		if (Vector3f(ev_odom.position).isAllFinite()) {
+		const Vector3f ev_odom_pos(ev_odom.position);
-			bool position_valid = true;
+		const Vector3f ev_odom_pos_var(ev_odom.position_variance);
 
-			// switch (ev_odom.pose_frame) {
+		if (ev_odom_pos.isAllFinite()) {
-			// case vehicle_odometry_s::POSE_FRAME_NED:
+			bool position_frame_valid = false;
-			// 	ev_data.pos_frame = PositionFrame::LOCAL_FRAME_NED;
-			// 	break;
 
-			// case vehicle_odometry_s::POSE_FRAME_FRD:
+			switch (ev_odom.pose_frame) {
-			// 	ev_data.pos_frame = PositionFrame::LOCAL_FRAME_FRD;
+			case vehicle_odometry_s::POSE_FRAME_NED:
-			// 	break;
+				ev_data.pos_frame = PositionFrame::LOCAL_FRAME_NED;
+				position_frame_valid = true;
+				break;
 
-			// default:
+			case vehicle_odometry_s::POSE_FRAME_FRD:
-			// 	position_valid = false;
+				ev_data.pos_frame = PositionFrame::LOCAL_FRAME_FRD;
-			// 	break;
+				position_frame_valid = true;
-			// }
+				break;
+			}
 
-			if (position_valid) {
+			if (position_frame_valid) {
-				ev_data.pos(0) = ev_odom.position[0];
+				ev_data.pos = ev_odom_pos;
-				ev_data.pos(1) = ev_odom.position[1];
-				ev_data.pos(2) = ev_odom.position[2];
 
 				const float evp_noise_var = sq(_param_ekf2_evp_noise.get());
 
 				// position measurement error from ev_data or parameters
-				if (!_param_ekf2_ev_noise_md.get() && Vector3f(ev_odom.position_variance).isAllFinite()) {
+				if ((_param_ekf2_ev_noise_md.get() == 0) && ev_odom_pos_var.isAllFinite()) {
-					ev_data.posVar(0) = fmaxf(evp_noise_var, ev_odom.position_variance[0]);
+
-					ev_data.posVar(1) = fmaxf(evp_noise_var, ev_odom.position_variance[1]);
+					ev_data.position_var(0) = fmaxf(evp_noise_var, ev_odom_pos_var(0));
-					ev_data.posVar(2) = fmaxf(evp_noise_var, ev_odom.position_variance[2]);
+					ev_data.position_var(1) = fmaxf(evp_noise_var, ev_odom_pos_var(1));
+					ev_data.position_var(2) = fmaxf(evp_noise_var, ev_odom_pos_var(2));
 
 				} else {
-					ev_data.posVar.setAll(evp_noise_var);
+					ev_data.position_var.setAll(evp_noise_var);
 				}
 
 				new_ev_odom = true;

src/modules/ekf2/EKF2.hpp

@@ -69,6 +69,7 @@
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/ekf2_timestamps.h>
 #include <uORB/topics/estimator_bias.h>
+#include <uORB/topics/estimator_bias3d.h>
 #include <uORB/topics/estimator_event_flags.h>
 #include <uORB/topics/estimator_gps_status.h>
 #include <uORB/topics/estimator_innovations.h>
@@ -142,7 +143,7 @@ private:
 	void PublishBaroBias(const hrt_abstime &timestamp);
 	void PublishGnssHgtBias(const hrt_abstime &timestamp);
 	void PublishRngHgtBias(const hrt_abstime &timestamp);
-	void PublishEvHgtBias(const hrt_abstime &timestamp);
+	void PublishEvPosBias(const hrt_abstime &timestamp);
 	estimator_bias_s fillEstimatorBiasMsg(const BiasEstimator::status &status, uint64_t timestamp_sample_us,
 					      uint64_t timestamp, uint32_t device_id = 0);
 	void PublishEventFlags(const hrt_abstime &timestamp);
@@ -165,7 +166,7 @@ private:
 	void UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps);
-	bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odometry_s &ev_odom);
+	bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps);
 	bool UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps);
@@ -291,7 +292,7 @@ private:
 	float _last_baro_bias_published{};
 	float _last_gnss_hgt_bias_published{};
 	float _last_rng_hgt_bias_published{};
-	float _last_ev_hgt_bias_published{};
+	matrix::Vector3f _last_ev_bias_published{};
 
 	float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements
 	hrt_abstime _airspeed_validated_timestamp_last{0};
@@ -340,7 +341,7 @@ private:
 	uORB::PublicationMulti<estimator_bias_s>             _estimator_baro_bias_pub{ORB_ID(estimator_baro_bias)};
 	uORB::PublicationMulti<estimator_bias_s>             _estimator_gnss_hgt_bias_pub{ORB_ID(estimator_gnss_hgt_bias)};
 	uORB::PublicationMulti<estimator_bias_s>             _estimator_rng_hgt_bias_pub{ORB_ID(estimator_rng_hgt_bias)};
-	uORB::PublicationMulti<estimator_bias_s>             _estimator_ev_hgt_bias_pub{ORB_ID(estimator_ev_hgt_bias)};
+	uORB::PublicationMulti<estimator_bias3d_s>           _estimator_ev_pos_bias_pub{ORB_ID(estimator_ev_pos_bias)};
 	uORB::PublicationMultiData<estimator_event_flags_s>  _estimator_event_flags_pub{ORB_ID(estimator_event_flags)};
 	uORB::PublicationMulti<estimator_gps_status_s>       _estimator_gps_status_pub{ORB_ID(estimator_gps_status)};
 	uORB::PublicationMulti<estimator_innovations_s>      _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
@@ -350,7 +351,6 @@ private:
 	uORB::PublicationMulti<estimator_states_s>           _estimator_states_pub{ORB_ID(estimator_states)};
 	uORB::PublicationMulti<estimator_status_flags_s>     _estimator_status_flags_pub{ORB_ID(estimator_status_flags)};
 	uORB::PublicationMulti<estimator_status_s>           _estimator_status_pub{ORB_ID(estimator_status)};
-	uORB::PublicationMulti<vehicle_odometry_s>           _estimator_visual_odometry_aligned_pub{ORB_ID(estimator_visual_odometry_aligned)};
 	uORB::PublicationMulti<vehicle_optical_flow_vel_s> _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
 	uORB::PublicationMulti<yaw_estimator_status_s>       _yaw_est_pub{ORB_ID(yaw_estimator_status)};
 

src/modules/ekf2/ekf2_params.c

@@ -603,10 +603,10 @@ PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 3.0f);
  * 0 : Deprecated, use EKF2_GPS_CTRL instead
  * 1 : Set to true to use optical flow data if available
  * 2 : Set to true to inhibit IMU delta velocity bias estimation
- * 3 : Set to true to enable vision position fusion
+ * 3 : Deprecated, use EKF2_EV_CTRL instead
- * 4 : Set to true to enable vision yaw fusion. Cannot be used if bit position 7 is true.
+ * 4 : Deprecated, use EKF2_EV_CTRL instead
  * 5 : Set to true to enable multi-rotor drag specific force fusion
- * 6 : set to true if the EV observations are in a non NED reference frame and need to be rotated before being used
+ * 6 : Deprecated, use EKF2_EV_CTRL instead
  * 7 : Deprecated, use EKF2_GPS_CTRL instead
  * 3 : Deprecated, use EKF2_EV_CTRL instead
  *
@@ -616,10 +616,10 @@ PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 3.0f);
  * @bit 0 unused
  * @bit 1 use optical flow
  * @bit 2 inhibit IMU bias estimation
- * @bit 3 vision position fusion
+ * @bit 3 unused
- * @bit 4 vision yaw fusion
+ * @bit 4 unused
  * @bit 5 multi-rotor drag fusion
- * @bit 6 rotate external vision
+ * @bit 6 unused
  * @bit 7 unused
  * @bit 8 unused
  * @reboot_required true
@@ -841,7 +841,7 @@ PARAM_DEFINE_FLOAT(EKF2_EVV_NOISE, 0.1f);
  * @unit rad
  * @decimal 2
  */
-PARAM_DEFINE_FLOAT(EKF2_EVA_NOISE, 0.05f);
+PARAM_DEFINE_FLOAT(EKF2_EVA_NOISE, 0.1f);
 
 /**
  * Measurement noise for the optical flow sensor when it's reported quality metric is at the maximum

src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp

@@ -137,12 +137,12 @@ void EkfWrapper::setFlowOffset(const Vector3f &offset)
 
 void EkfWrapper::enableExternalVisionPositionFusion()
 {
-	_ekf_params->fusion_mode |= SensorFusionMask::USE_EXT_VIS_POS;
+	_ekf_params->ev_ctrl |= static_cast<int32_t>(EvCtrl::HPOS);
 }
 
 void EkfWrapper::disableExternalVisionPositionFusion()
 {
-	_ekf_params->fusion_mode &= ~SensorFusionMask::USE_EXT_VIS_POS;
+	_ekf_params->ev_ctrl &= ~static_cast<int32_t>(EvCtrl::HPOS);
 }
 
 bool EkfWrapper::isIntendingExternalVisionPositionFusion() const
@@ -167,12 +167,12 @@ bool EkfWrapper::isIntendingExternalVisionVelocityFusion() const
 
 void EkfWrapper::enableExternalVisionHeadingFusion()
 {
-	_ekf_params->fusion_mode |= SensorFusionMask::USE_EXT_VIS_YAW;
+	_ekf_params->ev_ctrl |= static_cast<int32_t>(EvCtrl::YAW);
 }
 
 void EkfWrapper::disableExternalVisionHeadingFusion()
 {
-	_ekf_params->fusion_mode &= ~SensorFusionMask::USE_EXT_VIS_YAW;
+	_ekf_params->ev_ctrl &= ~static_cast<int32_t>(EvCtrl::YAW);
 }
 
 bool EkfWrapper::isIntendingExternalVisionHeadingFusion() const
@@ -180,16 +180,6 @@ bool EkfWrapper::isIntendingExternalVisionHeadingFusion() const
 	return _ekf->control_status_flags().ev_yaw;
 }
 
-void EkfWrapper::enableExternalVisionAlignment()
-{
-	_ekf_params->fusion_mode |= SensorFusionMask::ROTATE_EXT_VIS;
-}
-
-void EkfWrapper::disableExternalVisionAlignment()
-{
-	_ekf_params->fusion_mode &= ~SensorFusionMask::ROTATE_EXT_VIS;
-}
-
 bool EkfWrapper::isIntendingMagHeadingFusion() const
 {
 	return _ekf->control_status_flags().mag_hdg;

src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h

@@ -98,9 +98,6 @@ public:
 	bool isIntendingMag3DFusion() const;
 	void setMagFuseTypeNone();
 
-	void enableExternalVisionAlignment();
-	void disableExternalVisionAlignment();
-
 	bool isWindVelocityEstimated() const;
 
 	void enableTerrainRngFusion();

src/modules/ekf2/test/sensor_simulator/vio.cpp

@@ -8,6 +8,7 @@ namespace sensor
 Vio::Vio(std::shared_ptr<Ekf> ekf): Sensor(ekf)
 {
 	_vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD;
+	_vio_data.pos_frame = PositionFrame::LOCAL_FRAME_FRD;
 }
 
 Vio::~Vio()
@@ -32,7 +33,7 @@ void Vio::setVelocityVariance(const Vector3f &velVar)
 
 void Vio::setPositionVariance(const Vector3f &posVar)
 {
-	_vio_data.posVar = posVar;
+	_vio_data.position_var = posVar;
 }
 
 void Vio::setAngularVariance(float angVar)
@@ -70,16 +71,27 @@ void Vio::setVelocityFrameToLocalNED()
 	_vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_NED;
 }
 
+void Vio::setPositionFrameToLocalNED()
+{
+	_vio_data.pos_frame = PositionFrame::LOCAL_FRAME_NED;
+}
+
+void Vio::setPositionFrameToLocalFRD()
+{
+	_vio_data.pos_frame = PositionFrame::LOCAL_FRAME_FRD;
+}
+
 extVisionSample Vio::dataAtRest()
 {
 	extVisionSample vio_data;
-	vio_data.pos = Vector3f{0.0f, 0.0f, 0.0f};;
+	vio_data.pos = Vector3f{0.0f, 0.0f, 0.0f};
-	vio_data.vel = Vector3f{0.0f, 0.0f, 0.0f};;
+	vio_data.vel = Vector3f{0.0f, 0.0f, 0.0f};
 	vio_data.quat = Quatf{1.0f, 0.0f, 0.0f, 0.0f};
-	vio_data.posVar = Vector3f{0.1f, 0.1f, 0.1f};
+	vio_data.position_var = Vector3f{0.1f, 0.1f, 0.1f};
 	vio_data.velocity_var = Vector3f{0.1f, 0.1f, 0.1f};
 	vio_data.orientation_var(2) = 0.05f;
 	vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD;
+	vio_data.pos_frame = PositionFrame::LOCAL_FRAME_FRD;
 	return vio_data;
 }
 

src/modules/ekf2/test/sensor_simulator/vio.h

@@ -63,6 +63,9 @@ public:
 	void setVelocityFrameToLocalFRD();
 	void setVelocityFrameToBody();
 
+	void setPositionFrameToLocalNED();
+	void setPositionFrameToLocalFRD();
+
 	extVisionSample dataAtRest();
 
 private:

src/modules/ekf2/test/test_EKF_externalVision.cpp

@@ -78,6 +78,7 @@ TEST_F(EkfExternalVisionTest, checkVisionFusionLogic)
 {
 	_sensor_simulator.runSeconds(_tilt_align_time); // Let the tilt align
 	_ekf_wrapper.enableExternalVisionPositionFusion();
+	_sensor_simulator._vio.setPositionFrameToLocalNED();
 	_sensor_simulator.startExternalVision();
 	_sensor_simulator.runSeconds(2);
 
@@ -147,7 +148,6 @@ TEST_F(EkfExternalVisionTest, visionVelocityResetWithAlignment)
 	// WHEN: Vision frame is rotate +90°. The reported heading is -90°
 	Quatf vision_to_ekf(Eulerf(0.0f, 0.0f, math::radians(-90.0f)));
 	_sensor_simulator._vio.setOrientation(vision_to_ekf.inversed());
-	_ekf_wrapper.enableExternalVisionAlignment();
 
 	const Vector3f simulated_velocity_in_vision_frame(0.3f, -1.0f, 0.4f);
 	const Vector3f simulated_velocity_in_ekf_frame =
@@ -179,6 +179,7 @@ TEST_F(EkfExternalVisionTest, visionHorizontalPositionReset)
 	const Vector3f simulated_position(8.3f, -1.0f, 0.0f);
 
 	_sensor_simulator._vio.setPosition(simulated_position);
+	_sensor_simulator._vio.setPositionFrameToLocalNED();
 	_ekf_wrapper.enableExternalVisionPositionFusion();
 	_sensor_simulator.startExternalVision();
 	_sensor_simulator.runMicroseconds(2e5);
@@ -197,7 +198,6 @@ TEST_F(EkfExternalVisionTest, visionHorizontalPositionResetWithAlignment)
 	// WHEN: Vision frame is rotate +90°. The reported heading is -90°
 	Quatf vision_to_ekf(Eulerf(0.0f, 0.0f, math::radians(-90.0f)));
 	_sensor_simulator._vio.setOrientation(vision_to_ekf.inversed());
-	_ekf_wrapper.enableExternalVisionAlignment();
 
 	const Vector3f simulated_position_in_vision_frame(8.3f, -1.0f, 0.0f);
 	const Vector3f simulated_position_in_ekf_frame =
@@ -236,7 +236,6 @@ TEST_F(EkfExternalVisionTest, visionAlignment)
 	// WHEN: Vision frame is rotate +90°. The reported heading is -90°
 	Quatf externalVisionFrameOffset(Eulerf(0.0f, 0.0f, math::radians(90.0f)));
 	_sensor_simulator._vio.setOrientation(externalVisionFrameOffset.inversed());
-	_ekf_wrapper.enableExternalVisionAlignment();
 
 	// Simulate high uncertainty on vision x axis which is in this case
 	// the y EKF frame axis
@@ -254,9 +253,9 @@ TEST_F(EkfExternalVisionTest, visionAlignment)
 	EXPECT_TRUE(velVar_new(1) > velVar_new(0));
 
 	// THEN: the frame offset should be estimated correctly
-	Quatf estimatedExternalVisionFrameOffset = _ekf->getVisionAlignmentQuaternion();
+	// Quatf estimatedExternalVisionFrameOffset = _ekf->getVisionAlignmentQuaternion();
-	EXPECT_TRUE(matrix::isEqual(externalVisionFrameOffset.canonical(),
+	// EXPECT_TRUE(matrix::isEqual(externalVisionFrameOffset.canonical(),
-				    estimatedExternalVisionFrameOffset.canonical()));
+	// 			    estimatedExternalVisionFrameOffset.canonical()));
 }
 
 TEST_F(EkfExternalVisionTest, velocityFrameBody)
@@ -340,7 +339,6 @@ TEST_F(EkfExternalVisionTest, positionFrameLocal)
 	// WHEN: using EV yaw fusion and rotate EV is set
 	Quatf vision_to_ekf(Eulerf(0.0f, 0.0f, 0.f));
 	_sensor_simulator._vio.setOrientation(vision_to_ekf.inversed());
-	_ekf_wrapper.enableExternalVisionAlignment(); // ROTATE_EV
 	_ekf_wrapper.enableExternalVisionHeadingFusion(); // EV_YAW
 
 	// AND WHEN: using EV position aiding

src/modules/logger/logged_topics.cpp

@@ -149,7 +149,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("estimator_baro_bias", 500);
 	add_topic("estimator_gnss_hgt_bias", 500);
 	add_topic("estimator_rng_hgt_bias", 500);
-	add_topic("estimator_ev_hgt_bias", 500);
+	add_topic("estimator_ev_pos_bias", 500);
 	add_topic("estimator_event_flags", 0);
 	add_topic("estimator_gps_status", 1000);
 	add_topic("estimator_innovation_test_ratios", 500);
@@ -160,13 +160,12 @@ void LoggedTopics::add_default_topics()
 	add_topic("estimator_states", 1000);
 	add_topic("estimator_status", 200);
 	add_topic("estimator_status_flags", 0);
-	add_topic("estimator_visual_odometry_aligned", 200);
 	add_topic("yaw_estimator_status", 1000);
 
 	add_optional_topic_multi("estimator_baro_bias", 500, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_gnss_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_rng_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES);
-	add_optional_topic_multi("estimator_ev_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_ev_pos_bias", 500, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_event_flags", 0, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_gps_status", 1000, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_innovation_test_ratios", 500, MAX_ESTIMATOR_INSTANCES);
@@ -177,7 +176,6 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic_multi("estimator_states", 1000, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_status", 200, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_status_flags", 0, MAX_ESTIMATOR_INSTANCES);
-	add_optional_topic_multi("estimator_visual_odometry_aligned", 200, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("yaw_estimator_status", 1000, MAX_ESTIMATOR_INSTANCES);
 
 	// add_optional_topic_multi("estimator_aid_src_airspeed", 100, MAX_ESTIMATOR_INSTANCES);
@@ -248,7 +246,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("estimator_baro_bias");
 	add_topic("estimator_gnss_hgt_bias");
 	add_topic("estimator_rng_hgt_bias");
-	add_topic("estimator_ev_hgt_bias");
+	add_topic("estimator_ev_pos_bias");
 	add_topic("estimator_event_flags");
 	add_topic("estimator_gps_status");
 	add_topic("estimator_innovation_test_ratios");
@@ -259,7 +257,6 @@ void LoggedTopics::add_default_topics()
 	add_topic("estimator_states");
 	add_topic("estimator_status");
 	add_topic("estimator_status_flags");
-	add_topic("estimator_visual_odometry_aligned");
 	add_topic("vehicle_attitude");
 	add_topic("vehicle_global_position");
 	add_topic("vehicle_local_position");
@@ -281,7 +278,6 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic_multi("estimator_aid_src_gnss_yaw", 0, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_aid_src_mag_heading", 0, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_aid_src_mag", 0, MAX_ESTIMATOR_INSTANCES);
-	add_optional_topic_multi("estimator_aid_src_ev_yaw", 0, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_aid_src_optical_flow", 0, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_aid_src_sideslip", 0, MAX_ESTIMATOR_INSTANCES);
 

src/modules/mavlink/mavlink_main.h

@@ -502,7 +502,6 @@ public:
 
 	bool hash_check_enabled() const { return _param_mav_hash_chk_en.get(); }
 	bool forward_heartbeats_enabled() const { return _param_mav_hb_forw_en.get(); }
-	bool odometry_loopback_enabled() const { return _param_mav_odom_lp.get(); }
 
 	bool failure_injection_enabled() const { return _param_sys_failure_injection_enabled.get(); }
 
@@ -675,7 +674,6 @@ private:
 		(ParamBool<px4::params::MAV_FWDEXTSP>) _param_mav_fwdextsp,
 		(ParamBool<px4::params::MAV_HASH_CHK_EN>) _param_mav_hash_chk_en,
 		(ParamBool<px4::params::MAV_HB_FORW_EN>) _param_mav_hb_forw_en,
-		(ParamBool<px4::params::MAV_ODOM_LP>) _param_mav_odom_lp,
 		(ParamInt<px4::params::MAV_RADIO_TOUT>)      _param_mav_radio_timeout,
 		(ParamInt<px4::params::SYS_HITL>) _param_sys_hitl,
 		(ParamBool<px4::params::SYS_FAILURE_EN>) _param_sys_failure_injection_enabled

src/modules/mavlink/mavlink_params.c

@@ -142,17 +142,6 @@ PARAM_DEFINE_INT32(MAV_HASH_CHK_EN, 1);
  */
 PARAM_DEFINE_INT32(MAV_HB_FORW_EN, 1);
 
-/**
- * Activate ODOMETRY loopback.
- *
- * If set, it gets the data from 'vehicle_visual_odometry' instead of 'vehicle_odometry'
- * serving as a loopback of the received ODOMETRY messages on the Mavlink receiver.
- *
- * @boolean
- * @group MAVLink
- */
-PARAM_DEFINE_INT32(MAV_ODOM_LP, 0);
-
 /**
  * Timeout in seconds for the RADIO_STATUS reports coming in
  *

src/modules/mavlink/streams/ODOMETRY.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021-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
@@ -49,42 +49,20 @@ public:
 
 	unsigned get_size() override
 	{
-		if (_mavlink->odometry_loopback_enabled()) {
+		return _vehicle_odometry_sub.advertised() ? MAVLINK_MSG_ID_ODOMETRY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
-			return _vodom_sub.advertised() ? MAVLINK_MSG_ID_ODOMETRY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
-
-		} else {
-			return _odom_sub.advertised() ? MAVLINK_MSG_ID_ODOMETRY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
-		}
 	}
 
 private:
 	explicit MavlinkStreamOdometry(Mavlink *mavlink) : MavlinkStream(mavlink) {}
 
-	uORB::Subscription _odom_sub{ORB_ID(vehicle_odometry)};
+	uORB::Subscription _vehicle_odometry_sub{ORB_ID(vehicle_odometry)};
-	uORB::Subscription _vodom_sub{ORB_ID(vehicle_visual_odometry)};
 
 	bool send() override
 	{
 		vehicle_odometry_s odom;
-		// check if it is to send visual odometry loopback or not
-		bool odom_updated = false;
 
-		mavlink_odometry_t msg{};
+		if (_vehicle_odometry_sub.update(&odom)) {
-
+			mavlink_odometry_t msg{};
-		if (_mavlink->odometry_loopback_enabled()) {
-			odom_updated = _vodom_sub.update(&odom);
-
-			// source: external vision system
-			msg.estimator_type = MAV_ESTIMATOR_TYPE_VISION;
-
-		} else {
-			odom_updated = _odom_sub.update(&odom);
-
-			// source: PX4 estimator
-			msg.estimator_type = MAV_ESTIMATOR_TYPE_AUTOPILOT;
-		}
-
-		if (odom_updated) {
 			msg.time_usec = odom.timestamp_sample;
 
 			// set the frame_id according to the local frame of the data