ekf2: add estimator aid source status (EV pos, EV vel)

msg/estimator_aid_source_1d.msg

@@ -20,3 +20,4 @@ bool fused                   # true if the sample was successfully fused
 
 # TOPICS estimator_aid_source_1d
 # TOPICS estimator_aid_src_baro_hgt estimator_aid_src_rng_hgt
+# TOPICS estimator_aid_src_ev_yaw

msg/estimator_aid_source_3d.msg

@@ -20,3 +20,4 @@ bool[3] fused                # true if the sample was successfully fused
 
 # TOPICS estimator_aid_source_3d
 # TOPICS estimator_aid_src_gnss_pos estimator_aid_src_gnss_vel
+# TOPICS estimator_aid_src_ev_pos estimator_aid_src_ev_vel

src/modules/ekf2/CMakeLists.txt

@@ -56,6 +56,7 @@ px4_add_module(
 		EKF/ekf_helper.cpp
 		EKF/EKFGSF_yaw.cpp
 		EKF/estimator_interface.cpp
+		EKF/ev_fusion.cpp
 		EKF/fake_pos_control.cpp
 		EKF/gps_checks.cpp
 		EKF/gps_control.cpp

src/modules/ekf2/EKF/CMakeLists.txt

@@ -41,6 +41,7 @@ add_library(ecl_EKF
 	ekf_helper.cpp
 	EKFGSF_yaw.cpp
 	estimator_interface.cpp
+	ev_fusion.cpp
 	fake_pos_control.cpp
 	gps_checks.cpp
 	gps_control.cpp

src/modules/ekf2/EKF/common.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
+ *   Copyright (c) 2015-2022 Estimation and Control Library (ECL). All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions

src/modules/ekf2/EKF/control.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015-2020 Estimation and Control Library (ECL). All rights reserved.
+ *   Copyright (c) 2015-2022 Estimation and Control Library (ECL). All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -162,6 +162,63 @@ 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 (_ev_data_ready) {
+
+			// Use an incremental position fusion method for EV position data if GPS is also used
+			_fuse_hpos_as_odom = (_params.fusion_mode & SensorFusionMask::USE_GPS);
+
+			// 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::USE_EXT_VIS_YAW) && !_control_status.flags.ev_yaw) {
+
+				// 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);
+
+			} else {
+				_R_ev_to_ekf.setIdentity();
+			}
+
+			// 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;
+			const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
+
+			if ((_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) || _fuse_hpos_as_odom) {
+				// rotate measurement into body frame is required when fusing with GPS
+				_ev_sample_delayed.pos = _R_ev_to_ekf * (_ev_sample_delayed.pos - pos_offset_earth);
+
+				Matrix3f obs_var = matrix::diag(_ev_sample_delayed.posVar);
+				obs_var = _R_ev_to_ekf * obs_var * _R_ev_to_ekf.transpose();
+				_ev_sample_delayed.posVar = obs_var.diag();
+
+			} else {
+				_ev_sample_delayed.pos = (_ev_sample_delayed.pos - pos_offset_earth);
+			}
+
+			// rotate measurement into correct earth frame if required
+			switch (_ev_sample_delayed.vel_frame) {
+			case VelocityFrame::BODY_FRAME_FRD:
+				_ev_sample_delayed.vel = _R_to_earth * (_ev_sample_delayed.vel - vel_offset_body);
+				_ev_sample_delayed.velCov = _R_to_earth * _ev_sample_delayed.velCov * _R_to_earth.transpose();
+				break;
+
+			case VelocityFrame::LOCAL_FRAME_FRD:
+				const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
+
+				if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) {
+					_ev_sample_delayed.vel = _R_ev_to_ekf * (_ev_sample_delayed.vel - vel_offset_earth);
+					_ev_sample_delayed.velCov = _R_ev_to_ekf * _ev_sample_delayed.velCov * _R_ev_to_ekf.transpose();
+
+				} else {
+					_ev_sample_delayed.vel = _ev_sample_delayed.vel - vel_offset_earth;
+					// _ev_sample_delayed.velCov no change
+				}
+
+				break;
+			}
+		}
 	}
 
 	if (_airspeed_buffer) {
@@ -200,9 +257,21 @@ void Ekf::controlFusionModes()
 
 void Ekf::controlExternalVisionFusion()
 {
+	if (!_control_status_prev.flags.gps && _control_status.flags.gps) {
+		// GPS now enabled
+		stopEvYawFusion();
+
+		_fuse_hpos_as_odom = true;
+		_hpos_prev_available = false;
+	}
+
 	// Check for new external vision data
 	if (_ev_data_ready) {
 
+		// reset flags
+		resetEstimatorAidStatusFlags(_aid_src_ev_vel);
+		resetEstimatorAidStatusFlags(_aid_src_ev_pos);
+
 		bool reset = false;
 
 		if (_ev_sample_delayed.reset_counter != _ev_sample_delayed_prev.reset_counter) {
@@ -213,15 +282,6 @@ void Ekf::controlExternalVisionFusion()
 			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.fusion_mode & SensorFusionMask::USE_EXT_VIS_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) {
 
@@ -241,8 +301,10 @@ void Ekf::controlExternalVisionFusion()
 
 		// 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) {
+		    && _control_status.flags.tilt_align
-
+		    && !_control_status.flags.ev_yaw
+		    && !_control_status.flags.gps
+		) {
 			// don't start using EV data unless data is arriving frequently
 			if (isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
 				if (resetYawToEv()) {
@@ -260,57 +322,7 @@ void Ekf::controlExternalVisionFusion()
 				}
 			}
 
-			Vector3f ev_pos_obs_var;
+			if (!_fuse_hpos_as_odom) {
-
-			// 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.fusion_mode & SensorFusionMask::USE_GPS) {
-				_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
-					_ev_pos_innov(0) = _state.pos(0) - _hpos_pred_prev(0) - ev_delta_pos(0);
-					_ev_pos_innov(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));
-				}
-			} 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();
-				}
-
-				_ev_pos_innov(0) = _state.pos(0) - ev_pos_meas(0);
-				_ev_pos_innov(1) = _state.pos(1) - ev_pos_meas(1);
-
-				ev_pos_obs_var(0) = fmaxf(ev_pos_var(0, 0), sq(0.01f));
-				ev_pos_obs_var(1) = fmaxf(ev_pos_var(1, 1), sq(0.01f));
-
 				// check if we have been deadreckoning too long
 				if (isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)) {
 					// only reset velocity if we have no another source of aiding constraining it
@@ -325,11 +337,6 @@ void Ekf::controlExternalVisionFusion()
 					resetHorizontalPositionToVision();
 				}
 			}
-
-			// innovation gate size
-			const float ev_pos_innov_gate = fmaxf(_params.ev_pos_innov_gate, 1.0f);
-
-			fuseHorizontalPosition(_ev_pos_innov, ev_pos_innov_gate, ev_pos_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio);
 		}
 
 		// determine if we should use the velocity observations
@@ -338,8 +345,6 @@ void Ekf::controlExternalVisionFusion()
 				resetVelocityToVision();
 			}
 
-			_ev_vel_innov = _state.vel - getVisionVelocityInEkfFrame();
-
 			// check if we have been deadreckoning too long
 			if (isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)) {
 				// only reset velocity if we have no another source of aiding constraining it
@@ -348,15 +353,20 @@ void Ekf::controlExternalVisionFusion()
 					resetVelocityToVision();
 				}
 			}
-
-			const Vector3f obs_var = matrix::max(getVisionVelocityVarianceInEkfFrame(), sq(0.05f));
-
-			const float innov_gate = fmaxf(_params.ev_vel_innov_gate, 1.f);
-
-			fuseHorizontalVelocity(_ev_vel_innov, innov_gate, obs_var, _ev_vel_innov_var, _ev_vel_test_ratio);
-			fuseVerticalVelocity(_ev_vel_innov, innov_gate, obs_var, _ev_vel_innov_var, _ev_vel_test_ratio);
 		}
 
+		updateEvVel(_ev_sample_delayed);
+		updateEvPos(_ev_sample_delayed);
+
+		if (_control_status.flags.ev_vel) {
+			fuseEvVel();
+		}
+
+		if (_control_status.flags.ev_pos) {
+			fuseEvPos();
+		}
+
+		// External Vision Yaw
 		// determine if we should use the yaw observation
 		if (_control_status.flags.ev_yaw) {
 			if (reset && _control_status_prev.flags.ev_yaw) {
@@ -375,7 +385,8 @@ void Ekf::controlExternalVisionFusion()
 
 		// record observation and estimate for use next time
 		_ev_sample_delayed_prev = _ev_sample_delayed;
-		_hpos_pred_prev = _state.pos.xy();
+		_hpos_pred_prev = _state.pos;
+		_hpos_prev_available = true;
 
 	} else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel ||  _control_status.flags.ev_yaw)
 		   && isTimedOut(_time_last_ext_vision, (uint64_t)_params.reset_timeout_max)) {
@@ -966,13 +977,6 @@ void Ekf::controlHeightFusion()
 			fuseRngHgt(_aid_src_rng_hgt);
 		}
 	}
-
-	if (_control_status.flags.ev_hgt) {
-
-		if (_control_status.flags.ev_hgt && _ev_data_ready) {
-			fuseEvHgt();
-		}
-	}
 }
 
 void Ekf::checkRangeAidSuitability()

src/modules/ekf2/EKF/covariance.cpp

@@ -988,13 +988,13 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 		if (fabsf(down_dvel_bias) > dVel_bias_lim) {
 
 			bool bad_vz_gps = _control_status.flags.gps    && (down_dvel_bias * _aid_src_gnss_vel.innovation[2] < 0.0f);
-			bool bad_vz_ev  = _control_status.flags.ev_vel && (down_dvel_bias * _ev_vel_innov(2) < 0.0f);
+			bool bad_vz_ev  = _control_status.flags.ev_vel && (down_dvel_bias * _aid_src_ev_vel.innovation[2] < 0.0f);
 
 			if (bad_vz_gps || bad_vz_ev) {
 				bool bad_z_baro = _control_status.flags.baro_hgt && (down_dvel_bias * _aid_src_baro_hgt.innovation    < 0.0f);
 				bool bad_z_gps  = _control_status.flags.gps_hgt  && (down_dvel_bias * _aid_src_gnss_pos.innovation[2] < 0.0f);
 				bool bad_z_rng  = _control_status.flags.rng_hgt  && (down_dvel_bias * _aid_src_rng_hgt.innovation     < 0.0f);
-				bool bad_z_ev   = _control_status.flags.ev_hgt   && (down_dvel_bias * _ev_pos_innov(2) < 0.0f);
+				bool bad_z_ev   = _control_status.flags.ev_hgt   && (down_dvel_bias * _aid_src_ev_pos.innovation[2] < 0.0f);
 
 
 				if (bad_z_baro || bad_z_gps || bad_z_rng || bad_z_ev) {

src/modules/ekf2/EKF/ekf.h

@@ -348,6 +348,9 @@ public:
 	const auto &aid_src_gnss_vel() const { return _aid_src_gnss_vel; }
 	const auto &aid_src_gnss_pos() const { return _aid_src_gnss_pos; }
 
+	const auto &aid_src_ev_vel() const { return _aid_src_gnss_vel; }
+	const auto &aid_src_ev_pos() const { return _aid_src_gnss_pos; }
+
 private:
 
 	// set the internal states and status to their default value
@@ -381,7 +384,7 @@ private:
 
 	// variables used when position data is being fused using a relative position odometry model
 	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)
+	Vector3f _hpos_pred_prev{};		///< previous value of NE position 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)
@@ -450,12 +453,6 @@ private:
 	float _vert_vel_innov_ratio{0.f};		///< standard deviation of vertical velocity innovation
 	uint64_t _vert_vel_fuse_time_us{0};	///< last system time in usec time vertical velocity measurement fuson was attempted
 
-	Vector3f _ev_vel_innov{};	///< external vision velocity innovations (m/sec)
-	Vector3f _ev_vel_innov_var{};	///< external vision velocity innovation variances ((m/sec)**2)
-
-	Vector3f _ev_pos_innov{};	///< external vision position innovations (m)
-	Vector3f _ev_pos_innov_var{};	///< external vision position innovation variances (m**2)
-
 	Vector3f _aux_vel_innov{};	///< horizontal auxiliary velocity innovations: (m/sec)
 	Vector3f _aux_vel_innov_var{};	///< horizontal auxiliary velocity innovation variances: ((m/sec)**2)
 
@@ -499,6 +496,9 @@ private:
 	estimator_aid_source_3d_s _aid_src_gnss_vel{};
 	estimator_aid_source_3d_s _aid_src_gnss_pos{};
 
+	estimator_aid_source_3d_s _aid_src_ev_vel{};
+	estimator_aid_source_3d_s _aid_src_ev_pos{};
+
 	// output predictor states
 	Vector3f _delta_angle_corr{};	///< delta angle correction vector (rad)
 	Vector3f _vel_err_integ{};	///< integral of velocity tracking error (m)
@@ -647,7 +647,6 @@ private:
 
 	void fuseBaroHgt(estimator_aid_source_1d_s &baro_hgt);
 	void fuseRngHgt(estimator_aid_source_1d_s &range_hgt);
-	void fuseEvHgt();
 
 	void updateBaroHgt(const baroSample &baro_sample, estimator_aid_source_1d_s &baro_hgt);
 	void updateRngHgt(estimator_aid_source_1d_s &rng_hgt);
@@ -701,6 +700,12 @@ private:
 	void updateGpsPos(const gpsSample &gps_sample);
 	void fuseGpsPos();
 
+	void updateEvVel(const extVisionSample &ev_sample);
+	void fuseEvVel();
+
+	void updateEvPos(const extVisionSample &ev_sample);
+	void fuseEvPos();
+
 	// calculate optical flow body angular rate compensation
 	// returns false if bias corrected body rate data is unavailable
 	bool calcOptFlowBodyRateComp();
@@ -748,13 +753,6 @@ private:
 	// 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();
-
-	Vector3f getVisionVelocityInEkfFrame() const;
-
-	Vector3f getVisionVelocityVarianceInEkfFrame() const;
-
 	// matrix vector multiplication for computing K<24,1> * H<1,24> * P<24,24>
 	// that is optimized by exploring the sparsity in H
 	template <size_t ...Idxs>

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -87,8 +87,8 @@ void Ekf::resetVelocityToVision()
 {
 	_information_events.flags.reset_vel_to_vision = true;
 	ECL_INFO("reset to vision velocity");
-	resetVelocityTo(getVisionVelocityInEkfFrame());
+	resetVelocityTo(_ev_sample_delayed.vel);
-	P.uncorrelateCovarianceSetVariance<3>(4, getVisionVelocityVarianceInEkfFrame());
+	P.uncorrelateCovarianceSetVariance<3>(4, _ev_sample_delayed.velCov.diag());
 }
 
 void Ekf::resetHorizontalVelocityToZero()
@@ -156,13 +156,8 @@ 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) {
+	resetHorizontalPositionTo(Vector2f(_ev_sample_delayed.pos));
-		_ev_pos = _R_ev_to_ekf * _ev_sample_delayed.pos;
-	}
-
-	resetHorizontalPositionTo(Vector2f(_ev_pos));
 	P.uncorrelateCovarianceSetVariance<2>(7, _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
@@ -637,30 +632,33 @@ void Ekf::getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &v
 
 void Ekf::getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const
 {
-	hvel[0] = _ev_vel_innov(0);
+	hvel[0] = _aid_src_ev_vel.innovation[0];
-	hvel[1] = _ev_vel_innov(1);
+	hvel[1] = _aid_src_ev_vel.innovation[1];
-	vvel    = _ev_vel_innov(2);
+	vvel    = _aid_src_ev_vel.innovation[2];
-	hpos[0] = _ev_pos_innov(0);
+
-	hpos[1] = _ev_pos_innov(1);
+	hpos[0] = _aid_src_ev_pos.innovation[0];
-	vpos    = _ev_pos_innov(2);
+	hpos[1] = _aid_src_ev_pos.innovation[1];
+	vpos    = _aid_src_ev_pos.innovation[2];
 }
 
 void Ekf::getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const
 {
-	hvel[0] = _ev_vel_innov_var(0);
+	hvel[0] = _aid_src_ev_vel.innovation_variance[0];
-	hvel[1] = _ev_vel_innov_var(1);
+	hvel[1] = _aid_src_ev_vel.innovation_variance[1];
-	vvel    = _ev_vel_innov_var(2);
+	vvel    = _aid_src_ev_vel.innovation_variance[2];
-	hpos[0] = _ev_pos_innov_var(0);
+
-	hpos[1] = _ev_pos_innov_var(1);
+	hpos[0] = _aid_src_ev_pos.innovation_variance[0];
-	vpos    = _ev_pos_innov_var(2);
+	hpos[1] = _aid_src_ev_pos.innovation_variance[1];
+	vpos    = _aid_src_ev_pos.innovation_variance[2];
 }
 
 void Ekf::getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const
 {
-	hvel = _ev_vel_test_ratio(0);
+	hvel = fmaxf(_aid_src_ev_vel.test_ratio[0], _aid_src_ev_vel.test_ratio[1]);
-	vvel = _ev_vel_test_ratio(1);
+	vvel = _aid_src_ev_vel.test_ratio[2];
-	hpos = _ev_pos_test_ratio(0);
+
-	vpos = _ev_pos_test_ratio(1);
+	hpos = fmaxf(_aid_src_ev_pos.test_ratio[0], _aid_src_ev_pos.test_ratio[1]);
+	vpos = _aid_src_ev_pos.test_ratio[2];
 }
 
 void Ekf::getAuxVelInnov(float aux_vel_innov[2]) const
@@ -748,7 +746,7 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const
 		}
 
 		if (_control_status.flags.ev_pos) {
-			hpos_err = math::max(hpos_err, sqrtf(sq(_ev_pos_innov(0)) + sq(_ev_pos_innov(1))));
+			hpos_err = math::max(hpos_err, Vector2f(_aid_src_ev_pos.innovation).norm());
 		}
 	}
 
@@ -793,11 +791,11 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
 			vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_gnss_pos.innovation).norm());
 
 		} else if (_control_status.flags.ev_pos) {
-			vel_err_conservative = math::max(vel_err_conservative, sqrtf(sq(_ev_pos_innov(0)) + sq(_ev_pos_innov(1))));
+			vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_ev_pos.innovation).norm());
 		}
 
 		if (_control_status.flags.ev_vel) {
-			vel_err_conservative = math::max(vel_err_conservative, sqrtf(sq(_ev_vel_innov(0)) + sq(_ev_vel_innov(1))));
+			vel_err_conservative = math::max(vel_err_conservative, Vector3f(_aid_src_ev_vel.innovation).norm());
 		}
 
 		hvel_err = math::max(hvel_err, vel_err_conservative);
@@ -925,12 +923,12 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 	}
 
 	if (_control_status.flags.ev_vel) {
-		float ev_vel = sqrtf(math::max(_ev_vel_test_ratio(0), _ev_vel_test_ratio(1)));
+		float ev_vel = sqrtf(Vector3f(_aid_src_ev_vel.test_ratio).max());
 		vel = math::max(vel, ev_vel, FLT_MIN);
 	}
 
 	if (_control_status.flags.ev_pos) {
-		float ev_pos = sqrtf(_ev_pos_test_ratio(0));
+		float ev_pos = sqrtf(Vector2f(_aid_src_ev_pos.test_ratio).max());
 		pos = math::max(pos, ev_pos, FLT_MIN);
 	}
 
@@ -950,7 +948,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 		hgt = math::max(sqrtf(_aid_src_rng_hgt.test_ratio), FLT_MIN);
 
 	} else if (_control_status.flags.ev_hgt) {
-		hgt = math::max(sqrtf(_ev_pos_test_ratio(1)), FLT_MIN);
+		hgt = math::max(sqrtf(_aid_src_ev_pos.test_ratio[2]), FLT_MIN);
 
 	} else {
 		hgt = NAN;
@@ -1415,64 +1413,6 @@ void Ekf::updateGroundEffect()
 	}
 }
 
-Vector3f Ekf::getVisionVelocityInEkfFrame() const
-{
-	Vector3f vel;
-	// correct velocity for offset relative to IMU
-	const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
-	const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
-
-	// rotate measurement into correct earth frame if required
-	switch (_ev_sample_delayed.vel_frame) {
-	case VelocityFrame::BODY_FRAME_FRD:
-		vel = _R_to_earth * (_ev_sample_delayed.vel - vel_offset_body);
-		break;
-
-	case VelocityFrame::LOCAL_FRAME_FRD:
-		const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
-
-		if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) {
-			vel = _R_ev_to_ekf * _ev_sample_delayed.vel - vel_offset_earth;
-
-		} else {
-			vel = _ev_sample_delayed.vel - vel_offset_earth;
-		}
-
-		break;
-	}
-
-	return vel;
-}
-
-Vector3f Ekf::getVisionVelocityVarianceInEkfFrame() const
-{
-	Matrix3f ev_vel_cov = _ev_sample_delayed.velCov;
-
-	// rotate measurement into correct earth frame if required
-	switch (_ev_sample_delayed.vel_frame) {
-	case VelocityFrame::BODY_FRAME_FRD:
-		ev_vel_cov = _R_to_earth * ev_vel_cov * _R_to_earth.transpose();
-		break;
-
-	case VelocityFrame::LOCAL_FRAME_FRD:
-		if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) {
-			ev_vel_cov = _R_ev_to_ekf * ev_vel_cov * _R_ev_to_ekf.transpose();
-		}
-
-		break;
-	}
-
-	return ev_vel_cov.diag();
-}
-
-// update the rotation matrix which rotates EV measurements into the EKF's navigation frame
-void Ekf::calcExtVisRotMat()
-{
-	// 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)
@@ -1638,31 +1578,45 @@ void Ekf::stopGpsYawFusion()
 
 void Ekf::startEvPosFusion()
 {
-	_control_status.flags.ev_pos = true;
+	if (!_control_status.flags.ev_pos) {
-	resetHorizontalPositionToVision();
+		ECL_INFO("starting vision pos fusion");
-	_information_events.flags.starting_vision_pos_fusion = true;
+		_information_events.flags.starting_vision_pos_fusion = true;
-	ECL_INFO("starting vision pos fusion");
+
+		if (!_control_status.flags.gps) {
+			resetHorizontalPositionToVision();
+		}
+
+		_control_status.flags.ev_pos = true;
+	}
 }
 
 void Ekf::startEvVelFusion()
 {
-	_control_status.flags.ev_vel = true;
+	if (!_control_status.flags.ev_vel) {
-	resetVelocityToVision();
+		ECL_INFO("starting vision vel fusion");
-	_information_events.flags.starting_vision_vel_fusion = true;
+		_information_events.flags.starting_vision_vel_fusion = true;
-	ECL_INFO("starting vision vel fusion");
+
+		if (!_control_status.flags.gps) {
+			resetVelocityToVision();
+		}
+
+		_control_status.flags.ev_vel = true;
+	}
 }
 
 void Ekf::startEvYawFusion()
 {
-	// turn on fusion of external vision yaw measurements and disable all magnetometer fusion
+	if (!_control_status.flags.ev_yaw) {
-	_control_status.flags.ev_yaw = true;
+		// turn on fusion of external vision yaw measurements and disable all magnetometer fusion
-	_control_status.flags.mag_dec = false;
+		ECL_INFO("starting vision yaw fusion");
+		_information_events.flags.starting_vision_yaw_fusion = true;
 
-	stopMagHdgFusion();
+		_control_status.flags.ev_yaw = true;
-	stopMag3DFusion();
+		_control_status.flags.mag_dec = false;
 
-	_information_events.flags.starting_vision_yaw_fusion = true;
+		stopMagHdgFusion();
-	ECL_INFO("starting vision yaw fusion");
+		stopMag3DFusion();
+	}
 }
 
 void Ekf::stopEvFusion()
@@ -1674,23 +1628,28 @@ void Ekf::stopEvFusion()
 
 void Ekf::stopEvPosFusion()
 {
-	_control_status.flags.ev_pos = false;
+	if (_control_status.flags.ev_pos) {
-	_ev_pos_innov.setZero();
+		ECL_INFO("stopping EV position fusion");
-	_ev_pos_innov_var.setZero();
+		resetEstimatorAidStatus(_aid_src_ev_pos);
-	_ev_pos_test_ratio.setZero();
+		_control_status.flags.ev_pos = false;
+	}
 }
 
 void Ekf::stopEvVelFusion()
 {
-	_control_status.flags.ev_vel = false;
+	if (_control_status.flags.ev_vel) {
-	_ev_vel_innov.setZero();
+		ECL_INFO("stopping EV velocity fusion");
-	_ev_vel_innov_var.setZero();
+		resetEstimatorAidStatus(_aid_src_ev_vel);
-	_ev_vel_test_ratio.setZero();
+		_control_status.flags.ev_vel = false;
+	}
 }
 
 void Ekf::stopEvYawFusion()
 {
-	_control_status.flags.ev_yaw = false;
+	if (_control_status.flags.ev_yaw) {
+		ECL_INFO("stopping EV yaw fusion");
+		_control_status.flags.ev_yaw = false;
+	}
 }
 
 void Ekf::stopAuxVelFusion()

src/modules/ekf2/EKF/estimator_interface.h

@@ -331,8 +331,6 @@ protected:
 	float _yaw_test_ratio{};		// yaw innovation consistency check ratio
 	AlphaFilter<float> _yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state
 	Vector3f _mag_test_ratio{};		// magnetometer XYZ innovation consistency check ratios
-	Vector2f _ev_vel_test_ratio{};		// EV velocity innovation consistency check ratios
-	Vector2f _ev_pos_test_ratio{};		// EV position innovation consistency check ratios
 	Vector2f _aux_vel_test_ratio{};		// Auxiliary horizontal velocity innovation consistency check ratio
 	float _optflow_test_ratio{};		// Optical flow innovation consistency check ratio
 	float _tas_test_ratio{};		// tas innovation consistency check ratio

src/modules/ekf2/EKF/ev_fusion.cpp

@@ -0,0 +1,182 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4. 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_fusion.cpp
+ * Function for fusing external vision (EV) velocity and position measurements
+ */
+
+#include "ekf.h"
+
+void Ekf::updateEvVel(const extVisionSample &ev_sample)
+{
+	// innovation gate size
+	const float innov_gate = fmaxf(_params.ev_vel_innov_gate, 1.f);
+
+	auto &ev_vel = _aid_src_ev_vel;
+
+	for (int i = 0; i < 3; i++) {
+		ev_vel.observation[i] = ev_sample.vel(i);
+		ev_vel.observation_variance[i] = fmaxf(ev_sample.velCov(i, i), sq(0.05f));
+
+		ev_vel.innovation[i] = _state.vel(i) - ev_sample.vel(i);
+		ev_vel.innovation_variance[i] = P(4 + i, 4 + i) + ev_vel.observation_variance[i];
+		ev_vel.test_ratio[i] = sq(ev_vel.innovation[i]) / (sq(innov_gate) * ev_vel.innovation_variance[i]);
+
+		ev_vel.innovation_rejected[i] = (ev_vel.test_ratio[i] > 1.f);
+	}
+
+	// vz special case if there is bad vertical acceleration data, then don't reject measurement,
+	// but limit innovation to prevent spikes that could destabilise the filter
+	if (_fault_status.flags.bad_acc_vertical && ev_vel.innovation_rejected[2]) {
+		const float innov_limit = innov_gate * sqrtf(ev_vel.innovation_variance[2]);
+		ev_vel.innovation[2] = math::constrain(ev_vel.innovation[2], -innov_limit, innov_limit);
+		ev_vel.innovation_rejected[2] = false;
+	}
+
+	ev_vel.timestamp_sample = ev_sample.time_us;
+}
+
+void Ekf::updateEvPos(const extVisionSample &ev_sample)
+{
+	bool updated = false;
+
+	// innovation gate size
+	float innov_gate = fmaxf(_params.ev_pos_innov_gate, 1.f);
+
+	auto &ev_pos = _aid_src_ev_pos;
+
+	if (!_fuse_hpos_as_odom) {
+		// use the absolute position
+		for (int i = 0; i < 3; i++) {
+			ev_pos.observation[i] = _ev_sample_delayed.pos(i);
+			ev_pos.observation_variance[i] = fmaxf(_ev_sample_delayed.posVar(i), sq(0.01f));
+
+			ev_pos.innovation[i] = _state.pos(i) - _ev_sample_delayed.pos(i);
+		}
+
+		updated = true;
+
+	} else if (_hpos_prev_available) {
+		// calculate the change in position since the last measurement
+		Vector3f delta_pos = _ev_sample_delayed.pos - _ev_sample_delayed_prev.pos;
+
+		for (int i = 0; i < 3; i++) {
+			ev_pos.observation[i] = delta_pos(i);
+
+			// observation 1-STD error, incremental pos observation is expected to have more uncertainty
+			ev_pos.observation_variance[i] = fmaxf(_ev_sample_delayed.posVar(i), sq(0.5f));
+
+			ev_pos.innovation[i] = _state.pos(i) - _hpos_pred_prev(i) - delta_pos(i);
+		}
+
+		updated = true;
+	}
+
+	if (updated) {
+
+		for (int i = 0; i < 3; i++) {
+			ev_pos.innovation_variance[i] = P(7 + i, 7 + i) + ev_pos.observation_variance[i];
+			ev_pos.test_ratio[i] = sq(ev_pos.innovation[i]) / (sq(innov_gate) * ev_pos.innovation_variance[i]);
+
+			ev_pos.innovation_rejected[i] = (ev_pos.test_ratio[i] > 1.f);
+		}
+
+		// z special case if there is bad vertical acceleration data, then don't reject measurement,
+		// but limit innovation to prevent spikes that could destabilise the filter
+		if (_fault_status.flags.bad_acc_vertical && ev_pos.innovation_rejected[2]) {
+			const float innov_limit = innov_gate * sqrtf(ev_pos.innovation_variance[2]);
+			ev_pos.innovation[2] = math::constrain(ev_pos.innovation[2], -innov_limit, innov_limit);
+			ev_pos.innovation_rejected[2] = false;
+		}
+
+		ev_pos.timestamp_sample = ev_sample.time_us;
+	}
+}
+
+void Ekf::fuseEvVel()
+{
+	// velocity
+	auto &ev_vel = _aid_src_ev_vel;
+
+	// vx & vy
+	ev_vel.fusion_enabled[0] = true;
+	ev_vel.fusion_enabled[1] = true;
+
+	if (!ev_vel.innovation_rejected[0] && !ev_vel.innovation_rejected[1]) {
+		for (int i = 0; i < 2; i++) {
+			if (fuseVelPosHeight(ev_vel.innovation[i], ev_vel.innovation_variance[i], i)) {
+				ev_vel.fused[i] = true;
+				ev_vel.time_last_fuse[i] = _time_last_imu;
+			}
+		}
+	}
+
+	// vz
+	ev_vel.fusion_enabled[2] = true;
+
+	if (ev_vel.fusion_enabled[2] && !ev_vel.innovation_rejected[2]) {
+		if (fuseVelPosHeight(ev_vel.innovation[2], ev_vel.innovation_variance[2], 2)) {
+			ev_vel.fused[2] = true;
+			ev_vel.time_last_fuse[2] = _time_last_imu;
+		}
+	}
+}
+
+void Ekf::fuseEvPos()
+{
+	auto &ev_pos = _aid_src_ev_pos;
+
+	// x & y
+	ev_pos.fusion_enabled[0] = true;
+	ev_pos.fusion_enabled[1] = true;
+
+	if (!ev_pos.innovation_rejected[0] && !ev_pos.innovation_rejected[1]) {
+		for (int i = 0; i < 2; i++) {
+			if (fuseVelPosHeight(ev_pos.innovation[i], ev_pos.innovation_variance[i], 3 + i)) {
+				ev_pos.fused[i] = true;
+				ev_pos.time_last_fuse[i] = _time_last_imu;
+			}
+		}
+	}
+
+	// z
+	ev_pos.fusion_enabled[2] = _control_status.flags.ev_hgt;
+
+	if (ev_pos.fusion_enabled[2] && !ev_pos.innovation_rejected[2]) {
+		if (fuseVelPosHeight(ev_pos.innovation[2], ev_pos.innovation_variance[2], 2)) {
+			ev_pos.fused[2] = true;
+			ev_pos.time_last_fuse[2] = _time_last_imu;
+		}
+	}
+}

src/modules/ekf2/EKF/gps_fusion.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4. All rights reserved.
+ *   Copyright (c) 2021-2022 PX4. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions

src/modules/ekf2/EKF/height_fusion.cpp

@@ -142,19 +142,3 @@ void Ekf::fuseRngHgt(estimator_aid_source_1d_s &rng_hgt)
 		rng_hgt.time_last_fuse = _time_last_imu;
 	}
 }
-
-void Ekf::fuseEvHgt()
-{
-	// calculate the innovation assuming the external vision observation is in local NED frame
-	_ev_pos_innov(2) = _state.pos(2) - _ev_sample_delayed.pos(2);
-
-	// innovation gate size
-	float innov_gate = fmaxf(_params.ev_pos_innov_gate, 1.f);
-
-	// observation variance - defined externally
-	float obs_var = fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f));
-
-	// _ev_pos_test_ratio(1) is the vertical test ratio
-	fuseVerticalPosition(_ev_pos_innov(2), innov_gate, obs_var,
-			     _ev_pos_innov_var(2), _ev_pos_test_ratio(1));
-}

src/modules/ekf2/EKF2.cpp

@@ -649,6 +649,10 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
 	PublishAidSourceStatus(_ekf.aid_src_gnss_vel(), _status_gnss_vel_pub_last, _estimator_aid_src_gnss_vel_pub);
 	PublishAidSourceStatus(_ekf.aid_src_gnss_pos(), _status_gnss_pos_pub_last, _estimator_aid_src_gnss_pos_pub);
 
+	// EV velocity & position
+	PublishAidSourceStatus(_ekf.aid_src_ev_vel(), _status_ev_vel_pub_last, _estimator_aid_src_ev_vel_pub);
+	PublishAidSourceStatus(_ekf.aid_src_ev_pos(), _status_ev_pos_pub_last, _estimator_aid_src_ev_pos_pub);
+
 }
 
 void EKF2::PublishAttitude(const hrt_abstime &timestamp)

src/modules/ekf2/EKF2.hpp

@@ -263,6 +263,9 @@ private:
 	hrt_abstime _status_gnss_vel_pub_last{0};
 	hrt_abstime _status_gnss_pos_pub_last{0};
 
+	hrt_abstime _status_ev_vel_pub_last{0};
+	hrt_abstime _status_ev_pos_pub_last{0};
+
 	float _last_baro_bias_published{};
 
 	float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements
@@ -330,6 +333,9 @@ private:
 	uORB::PublicationMulti<estimator_aid_source_3d_s> _estimator_aid_src_gnss_vel_pub{ORB_ID(estimator_aid_src_gnss_vel)};
 	uORB::PublicationMulti<estimator_aid_source_3d_s> _estimator_aid_src_gnss_pos_pub{ORB_ID(estimator_aid_src_gnss_pos)};
 
+	uORB::PublicationMulti<estimator_aid_source_3d_s> _estimator_aid_src_ev_vel_pub{ORB_ID(estimator_aid_src_ev_vel)};
+	uORB::PublicationMulti<estimator_aid_source_3d_s> _estimator_aid_src_ev_pos_pub{ORB_ID(estimator_aid_src_ev_pos)};
+
 	// publications with topic dependent on multi-mode
 	uORB::PublicationMulti<vehicle_attitude_s>           _attitude_pub;
 	uORB::PublicationMulti<vehicle_local_position_s>     _local_position_pub;