ekf2: minor position/velocity reset cleanup

- try to use avoid resetVelocity() call where possible
 - reset timeouts centrally

src/modules/ekf2/EKF/control.cpp

@@ -106,6 +106,21 @@ void Ekf::controlFusionModes()
 		// check for arrival of new sensor data at the fusion time horizon
 		_time_prev_gps_us = _gps_sample_delayed.time_us;
 		_gps_data_ready = _gps_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed);
+
+		if (_gps_data_ready) {
+			// correct velocity for offset relative to IMU
+			const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
+			const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
+			const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
+			_gps_sample_delayed.vel -= vel_offset_earth;
+
+			// correct position and height for offset relative to IMU
+			const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
+			_gps_sample_delayed.pos -= pos_offset_earth.xy();
+			_gps_sample_delayed.hgt += pos_offset_earth(2);
+
+			_gps_sample_delayed.sacc = fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise);
+		}
 	}
 
 	if (_range_buffer) {
@@ -145,6 +160,9 @@ void Ekf::controlFusionModes()
 		_tas_data_ready = _airspeed_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed);
 	}
 
+	// run EKF-GSF yaw estimator once per _imu_sample_delayed update after all main EKF data samples available
+	runYawEKFGSF();
+
 	// check for height sensor timeouts and reset and change sensor if necessary
 	controlHeightSensorTimeouts();
 
@@ -227,7 +245,9 @@ void Ekf::controlExternalVisionFusion()
 		// determine if we should use the horizontal position observations
 		if (_control_status.flags.ev_pos) {
 			if (reset && _control_status_prev.flags.ev_pos) {
-				resetHorizontalPosition();
+				if (!_fuse_hpos_as_odom) {
+					resetHorizontalPositionToVision();
+				}
 			}
 
 			Vector3f ev_pos_obs_var;
@@ -286,10 +306,13 @@ void Ekf::controlExternalVisionFusion()
 					// only reset velocity if we have no another source of aiding constraining it
 					if (isTimedOut(_time_last_of_fuse, (uint64_t)1E6) &&
 					    isTimedOut(_time_last_hor_vel_fuse, (uint64_t)1E6)) {
-						resetVelocity();
+
+						if (_control_status.flags.ev_vel) {
+							resetVelocityToVision();
+						}
 					}
 
-					resetHorizontalPosition();
+					resetHorizontalPositionToVision();
 				}
 			}
 
@@ -302,7 +325,7 @@ void Ekf::controlExternalVisionFusion()
 		// determine if we should use the velocity observations
 		if (_control_status.flags.ev_vel) {
 			if (reset && _control_status_prev.flags.ev_vel) {
-				resetVelocity();
+				resetVelocityToVision();
 			}
 
 			_ev_vel_innov = _state.vel - getVisionVelocityInEkfFrame();
@@ -312,7 +335,7 @@ void Ekf::controlExternalVisionFusion()
 				// only reset velocity if we have no another source of aiding constraining it
 				if (isTimedOut(_time_last_of_fuse, (uint64_t)1E6) &&
 				    isTimedOut(_time_last_hor_pos_fuse, (uint64_t)1E6)) {
-					resetVelocity();
+					resetVelocityToVision();
 				}
 			}
 
@@ -460,8 +483,8 @@ void Ekf::controlOpticalFlowFusion()
 				const bool flow_aid_only = !isOtherSourceOfHorizontalAidingThan(_control_status.flags.opt_flow);
 
 				if (flow_aid_only) {
-					resetVelocity();
-					resetHorizontalPosition();
+					resetHorizontalVelocityToOpticalFlow();
+					resetHorizontalPositionToOpticalFlow();
 				}
 			}
 		}
@@ -483,8 +506,8 @@ void Ekf::controlOpticalFlowFusion()
 			if (isTimedOut(_time_last_of_fuse, _params.reset_timeout_max)
 			    && !isOtherSourceOfHorizontalAidingThan(_control_status.flags.opt_flow)) {
 
-				resetVelocity();
-				resetHorizontalPosition();
+				resetHorizontalVelocityToOpticalFlow();
+				resetHorizontalPositionToOpticalFlow();
 			}
 		}
 
@@ -726,7 +749,7 @@ void Ekf::controlHeightSensorTimeouts()
 
 		// Also reset the vertical velocity
 		if (_control_status.flags.gps && !_gps_intermittent && _gps_checks_passed) {
-			resetVerticalVelocityToGps();
+			resetVerticalVelocityToGps(_gps_sample_delayed);
 
 		} else {
 			resetVerticalVelocityToZero();
@@ -1068,16 +1091,3 @@ bool Ekf::hasHorizontalAidingTimedOut() const
 	       && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
 	       && isTimedOut(_time_last_of_fuse, _params.reset_timeout_max);
 }
-
-void Ekf::processVelPosResetRequest()
-{
-	if (_velpos_reset_request) {
-		resetVelocity();
-		resetHorizontalPosition();
-		_velpos_reset_request = false;
-
-		// Reset the timeout counters
-		_time_last_hor_pos_fuse = _time_last_imu;
-		_time_last_hor_vel_fuse = _time_last_imu;
-	}
-}

src/modules/ekf2/EKF/ekf.cpp

@@ -110,9 +110,6 @@ bool Ekf::update()
 		runTerrainEstimator();
 
 		updated = true;
-
-		// run EKF-GSF yaw estimator
-		runYawEKFGSF();
 	}
 
 	// the output observer always runs

src/modules/ekf2/EKF/ekf.h

@@ -327,12 +327,6 @@ private:
 
 	bool initialiseTilt();
 
-	// Request the EKF reset the yaw to the estimate from the internal EKF-GSF filter
-	// and reset the velocity and position states to the GPS. This will cause the EKF
-	// to ignore the magnetometer for the remainder of flight.
-	// This should only be used as a last resort before activating a loss of navigation failsafe
-	void requestEmergencyNavReset() { _do_ekfgsf_yaw_reset = true; }
-
 	// check if the EKF is dead reckoning horizontal velocity using inertial data only
 	void update_deadreckoning_status();
 
@@ -521,7 +515,6 @@ private:
 	uint64_t _time_last_mov_3d_mag_suitable{0};	///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec)
 	float _saved_mag_bf_variance[4] {};	///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2)
 	Matrix2f _saved_mag_ef_covmat{};		///< NE magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2)
-	bool _velpos_reset_request{false};	///< true when a large yaw error has been fixed and a velocity and position state reset is required
 
 	gps_check_fail_status_u _gps_check_fail_status{};
 
@@ -630,28 +623,19 @@ private:
 	// fuse single velocity and position measurement
 	bool fuseVelPosHeight(const float innov, const float innov_var, const int obs_index);
 
-	void resetVelocity();
-
-	void resetVelocityToGps();
-
-	void resetHorizontalVelocityToOpticalFlow();
-
-	void resetVelocityToVision();
-
-	void resetHorizontalVelocityToZero();
-
 	void resetVelocityTo(const Vector3f &vel);
-
 	void resetHorizontalVelocityTo(const Vector2f &new_horz_vel);
-
 	void resetVerticalVelocityTo(float new_vert_vel);
 
-	void resetHorizontalPosition();
-
-	void resetHorizontalPositionToGps();
+	void resetVelocityToGps(const gpsSample &gps_sample_delayed);
+	void resetHorizontalVelocityToOpticalFlow();
+	void resetVelocityToVision();
+	void resetHorizontalVelocityToZero();
 
+	void resetHorizontalPositionToGps(const gpsSample &gps_sample_delayed);
 	void resetHorizontalPositionToVision();
-
+	void resetHorizontalPositionToOpticalFlow();
+	void resetHorizontalPositionToLastKnown();
 	void resetHorizontalPositionTo(const Vector2f &new_horz_pos);
 
 	void resetVerticalPositionTo(float new_vert_pos);
@@ -661,7 +645,7 @@ private:
 	void resetHeightToRng();
 	void resetHeightToEv();
 
-	void resetVerticalVelocityToGps();
+	void resetVerticalVelocityToGps(const gpsSample &gps_sample_delayed);
 	void resetVerticalVelocityToZero();
 
 	// fuse optical flow line of sight rate measurements
@@ -834,8 +818,6 @@ private:
 	bool shouldResetGpsFusion() const;
 	bool hasHorizontalAidingTimedOut() const;
 	bool isYawFailure() const;
-	void processYawEstimatorResetRequest();
-	void processVelPosResetRequest();
 
 	void controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing);
 
@@ -852,8 +834,6 @@ private:
 	bool isYawResetAuthorized() const { return !_is_yaw_fusion_inhibited; }
 	bool canResetMagHeading() const;
 	void runInAirYawReset(const Vector3f &mag);
-	bool canRealignYawUsingGps() const { return _control_status.flags.fixed_wing; }
-	void runVelPosReset();
 
 	void selectMagAuto();
 	void check3DMagFusionSuitability();
@@ -1039,10 +1019,8 @@ private:
 	BaroBiasEstimator _baro_b_est{};
 
 	int64_t _ekfgsf_yaw_reset_time{0};	///< timestamp of last emergency yaw reset (uSec)
-	bool _do_ekfgsf_yaw_reset{false};	// true when an emergency yaw reset has been requested
 	uint8_t _ekfgsf_yaw_reset_count{0};	// number of times the yaw has been reset to the EKF-GSF estimate
 
-	// Call once per _imu_sample_delayed update after all main EKF data fusion oeprations have been completed
 	void runYawEKFGSF();
 
 	// Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -44,29 +44,12 @@
 #include <mathlib/mathlib.h>
 #include <cstdlib>
 
-void Ekf::resetVelocity()
-{
-	if (_control_status.flags.gps && isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us)) {
-		// this reset is only called if we have new gps data at the fusion time horizon
-		resetVelocityToGps();
-
-	} else if (_control_status.flags.opt_flow) {
-		resetHorizontalVelocityToOpticalFlow();
-
-	} else if (_control_status.flags.ev_vel) {
-		resetVelocityToVision();
-
-	} else {
-		resetHorizontalVelocityToZero();
-	}
-}
-
-void Ekf::resetVelocityToGps()
+void Ekf::resetVelocityToGps(const gpsSample &gps_sample_delayed)
 {
 	_information_events.flags.reset_vel_to_gps = true;
 	ECL_INFO("reset velocity to GPS");
-	resetVelocityTo(_gps_sample_delayed.vel);
-	P.uncorrelateCovarianceSetVariance<3>(4, sq(_gps_sample_delayed.sacc));
+	resetVelocityTo(gps_sample_delayed.vel);
+	P.uncorrelateCovarianceSetVariance<3>(4, sq(gps_sample_delayed.sacc));
 }
 
 void Ekf::resetHorizontalVelocityToOpticalFlow()
@@ -136,6 +119,9 @@ void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel)
 
 	_state_reset_status.velNE_change = delta_horz_vel;
 	_state_reset_status.velNE_counter++;
+
+	// Reset the timout timer
+	_time_last_hor_vel_fuse = _time_last_imu;
 }
 
 void Ekf::resetVerticalVelocityTo(float new_vert_vel)
@@ -153,48 +139,17 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel)
 
 	_state_reset_status.velD_change = delta_vert_vel;
 	_state_reset_status.velD_counter++;
+
+	// Reset the timout timer
+	_time_last_ver_vel_fuse = _time_last_imu;
 }
 
-void Ekf::resetHorizontalPosition()
-{
-	if (_control_status.flags.gps) {
-		// this reset is only called if we have new gps data at the fusion time horizon
-		resetHorizontalPositionToGps();
-
-	} else if (_control_status.flags.ev_pos) {
-		// this reset is only called if we have new ev data at the fusion time horizon
-		resetHorizontalPositionToVision();
-
-	} else if (_control_status.flags.opt_flow) {
-		_information_events.flags.reset_pos_to_last_known = true;
-		ECL_INFO("reset position to last known position");
-
-		if (!_control_status.flags.in_air) {
-			// we are likely starting OF for the first time so reset the horizontal position
-			resetHorizontalPositionTo(Vector2f(0.f, 0.f));
-
-		} else {
-			resetHorizontalPositionTo(_last_known_posNE);
-		}
-
-		// estimate is relative to initial position in this mode, so we start with zero error.
-		P.uncorrelateCovarianceSetVariance<2>(7, 0.0f);
-
-	} else {
-		_information_events.flags.reset_pos_to_last_known = true;
-		ECL_INFO("reset position to last known position");
-		// Used when falling back to non-aiding mode of operation
-		resetHorizontalPositionTo(_last_known_posNE);
-		P.uncorrelateCovarianceSetVariance<2>(7, sq(_params.pos_noaid_noise));
-	}
-}
-
-void Ekf::resetHorizontalPositionToGps()
+void Ekf::resetHorizontalPositionToGps(const gpsSample &gps_sample_delayed)
 {
 	_information_events.flags.reset_pos_to_gps = true;
 	ECL_INFO("reset position to GPS");
-	resetHorizontalPositionTo(_gps_sample_delayed.pos);
-	P.uncorrelateCovarianceSetVariance<2>(7, sq(_gps_sample_delayed.hacc));
+	resetHorizontalPositionTo(gps_sample_delayed.pos);
+	P.uncorrelateCovarianceSetVariance<2>(7, sq(gps_sample_delayed.hacc));
 }
 
 void Ekf::resetHorizontalPositionToVision()
@@ -214,6 +169,32 @@ void Ekf::resetHorizontalPositionToVision()
 	_hpos_prev_available = false;
 }
 
+void Ekf::resetHorizontalPositionToOpticalFlow()
+{
+	_information_events.flags.reset_pos_to_last_known = true;
+	ECL_INFO("reset position to last known position");
+
+	if (!_control_status.flags.in_air) {
+		// we are likely starting OF for the first time so reset the horizontal position
+		resetHorizontalPositionTo(Vector2f(0.f, 0.f));
+
+	} else {
+		resetHorizontalPositionTo(_last_known_posNE);
+	}
+
+	// estimate is relative to initial position in this mode, so we start with zero error.
+	P.uncorrelateCovarianceSetVariance<2>(7, 0.0f);
+}
+
+void Ekf::resetHorizontalPositionToLastKnown()
+{
+	_information_events.flags.reset_pos_to_last_known = true;
+	ECL_INFO("reset position to last known position");
+	// Used when falling back to non-aiding mode of operation
+	resetHorizontalPositionTo(_last_known_posNE);
+	P.uncorrelateCovarianceSetVariance<2>(7, sq(_params.pos_noaid_noise));
+}
+
 void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos)
 {
 	const Vector2f delta_horz_pos{new_horz_pos - Vector2f{_state.pos}};
@@ -227,6 +208,9 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos)
 
 	_state_reset_status.posNE_change = delta_horz_pos;
 	_state_reset_status.posNE_counter++;
+
+	// Reset the timout timer
+	_time_last_hor_pos_fuse = _time_last_imu;
 }
 
 void Ekf::resetVerticalPositionTo(const float new_vert_pos)
@@ -310,12 +294,12 @@ void Ekf::resetHeightToEv()
 	_baro_hgt_offset += _state.pos(2) - z_pos_before_reset;
 }
 
-void Ekf::resetVerticalVelocityToGps()
+void Ekf::resetVerticalVelocityToGps(const gpsSample &gps_sample_delayed)
 {
-	resetVerticalVelocityTo(_gps_sample_delayed.vel(2));
+	resetVerticalVelocityTo(gps_sample_delayed.vel(2));
 
 	// the state variance is the same as the observation
-	P.uncorrelateCovarianceSetVariance<1>(6, sq(1.5f * _gps_sample_delayed.sacc));
+	P.uncorrelateCovarianceSetVariance<1>(6, sq(1.5f * gps_sample_delayed.sacc));
 }
 
 void Ekf::resetVerticalVelocityToZero()
@@ -440,7 +424,10 @@ bool Ekf::realignYawGPS(const Vector3f &mag)
 		_flt_mag_align_start_time = _imu_sample_delayed.time_us;
 
 		// If heading was bad, then we also need to reset the velocity and position states
-		_velpos_reset_request = badMagYaw;
+		if (badMagYaw) {
+			resetVelocityToGps(_gps_sample_delayed);
+			resetHorizontalPositionToGps(_gps_sample_delayed);
+		}
 
 		return true;
 
@@ -1531,17 +1518,18 @@ void Ekf::stopAirspeedFusion()
 
 void Ekf::startGpsFusion()
 {
-	resetHorizontalPositionToGps();
+	if (!_control_status.flags.gps) {
+		resetHorizontalPositionToGps(_gps_sample_delayed);
 
-	// when using optical flow,
-	// velocity reset is not necessary
-	if (!_control_status.flags.opt_flow) {
-		resetVelocityToGps();
+		// when already using another velocity source velocity reset is not necessary
+		if (!_control_status.flags.opt_flow && !_control_status.flags.ev_vel) {
+			resetVelocityToGps(_gps_sample_delayed);
+		}
+
+		_information_events.flags.starting_gps_fusion = true;
+		ECL_INFO("starting GPS fusion");
+		_control_status.flags.gps = true;
 	}
-
-	_information_events.flags.starting_gps_fusion = true;
-	ECL_INFO("starting GPS fusion");
-	_control_status.flags.gps = true;
 }
 
 void Ekf::stopGpsFusion()
@@ -1601,7 +1589,7 @@ void Ekf::stopGpsYawFusion()
 void Ekf::startEvPosFusion()
 {
 	_control_status.flags.ev_pos = true;
-	resetHorizontalPosition();
+	resetHorizontalPositionToVision();
 	_information_events.flags.starting_vision_pos_fusion = true;
 	ECL_INFO("starting vision pos fusion");
 }
@@ -1609,7 +1597,7 @@ void Ekf::startEvPosFusion()
 void Ekf::startEvVelFusion()
 {
 	_control_status.flags.ev_vel = true;
-	resetVelocity();
+	resetVelocityToVision();
 	_information_events.flags.starting_vision_vel_fusion = true;
 	ECL_INFO("starting vision vel fusion");
 }
@@ -1722,42 +1710,27 @@ bool Ekf::resetYawToEKFGSF()
 
 	resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar(), true);
 
-	// reset velocity and position states to GPS - if yaw is fixed then the filter should start to operate correctly
-	resetVelocity();
-	resetHorizontalPosition();
-
 	// record a magnetic field alignment event to prevent possibility of the EKF trying to reset the yaw to the mag later in flight
 	_flt_mag_align_start_time = _imu_sample_delayed.time_us;
 	_control_status.flags.yaw_align = true;
 
-	const bool is_mag_fusion_active = _control_status.flags.mag_hdg
-	                                  || _control_status.flags.mag_3D;
-	const bool is_yaw_aiding_active = is_mag_fusion_active
-	                                  || _control_status.flags.gps_yaw
-					  || _control_status.flags.ev_yaw;
+	if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
+		// stop using the magnetometer in the main EKF otherwise it's fusion could drag the yaw around
+		// and cause another navigation failure
+		_control_status.flags.mag_fault = true;
+		_warning_events.flags.emergency_yaw_reset_mag_stopped = true;
 
-	if (!is_yaw_aiding_active) {
-		_information_events.flags.yaw_aligned_to_imu_gps = true;
-		ECL_INFO("Yaw aligned using IMU and GPS");
+	} else if (_control_status.flags.gps_yaw) {
+		_control_status.flags.gps_yaw_fault = true;
+		_warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = true;
 
-	} else {
-		if (is_mag_fusion_active) {
-			// stop using the magnetometer in the main EKF otherwise it's fusion could drag the yaw around
-			// and cause another navigation failure
-			_control_status.flags.mag_fault = true;
-			_warning_events.flags.emergency_yaw_reset_mag_stopped = true;
-
-		} else if (_control_status.flags.gps_yaw) {
-			_control_status.flags.gps_yaw_fault = true;
-			_warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = true;
-
-		} else if (_control_status.flags.ev_yaw) {
-			_inhibit_ev_yaw_use = true;
-		}
-
-		ECL_WARN("Emergency yaw reset");
+	} else if (_control_status.flags.ev_yaw) {
+		_inhibit_ev_yaw_use = true;
 	}
 
+	_ekfgsf_yaw_reset_time = _time_last_imu;
+	_ekfgsf_yaw_reset_count++;
+
 	return true;
 }
 

src/modules/ekf2/EKF/fake_pos_control.cpp

@@ -88,8 +88,8 @@ void Ekf::startFakePosFusion()
 void Ekf::resetFakePosFusion()
 {
 	_last_known_posNE = _state.pos.xy();
-	resetHorizontalPosition();
-	resetVelocity();
+	resetHorizontalPositionToLastKnown();
+	resetHorizontalVelocityToZero();
 	_time_last_fake_pos_fuse = _time_last_imu;
 }
 

src/modules/ekf2/EKF/gps_control.cpp

@@ -56,12 +56,11 @@ void Ekf::controlGpsFusion()
 		// 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 = _control_status.flags.tilt_align
-		                                          && _control_status.flags.yaw_align
-							  && _NED_origin_initialised;
-		const bool continuing_conditions_passing = mandatory_conditions_passing
-		                                           && !gps_checks_failing;
-		const bool starting_conditions_passing = continuing_conditions_passing
-							 && gps_checks_passing;
+				&& _control_status.flags.yaw_align
+				&& _NED_origin_initialised;
+
+		const bool continuing_conditions_passing = mandatory_conditions_passing && !gps_checks_failing;
+		const bool starting_conditions_passing = continuing_conditions_passing && gps_checks_passing;
 
 		if (_control_status.flags.gps) {
 			if (mandatory_conditions_passing) {
@@ -80,9 +79,13 @@ void Ekf::controlGpsFusion()
 						if (isYawFailure()
 						    && _control_status.flags.in_air
 						    && !was_gps_signal_lost
-						    && _ekfgsf_yaw_reset_count < _params.EKFGSF_reset_count_limit) {
-
-							_do_ekfgsf_yaw_reset = true;
+						    && _ekfgsf_yaw_reset_count < _params.EKFGSF_reset_count_limit
+						    && isTimedOut(_ekfgsf_yaw_reset_time, 5000000)) {
+							// The minimum time interval between resets to the EKF-GSF estimate is limited to allow the EKF-GSF time
+							// to improve its estimate if the previous reset was not successful.
+							if (resetYawToEKFGSF()) {
+								ECL_WARN("GPS emergency yaw reset");
+							}
 
 						} else {
 							// use GPS velocity data to check and correct yaw angle if a FW vehicle
@@ -93,8 +96,10 @@ void Ekf::controlGpsFusion()
 
 							_warning_events.flags.gps_fusion_timout = true;
 							ECL_WARN("GPS fusion timeout - resetting");
-							_velpos_reset_request = true;
 						}
+
+						resetVelocityToGps(_gps_sample_delayed);
+						resetHorizontalPositionToGps(_gps_sample_delayed);
 					}
 
 				} else {
@@ -105,7 +110,7 @@ void Ekf::controlGpsFusion()
 					// 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 & MASK_ROTATE_EV)) {
-						resetHorizontalPosition();
+						resetHorizontalPositionToVision();
 					}
 				}
 
@@ -131,15 +136,17 @@ void Ekf::controlGpsFusion()
 					startGpsFusion();
 				}
 
-			} else if (!_control_status.flags.yaw_align && (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE)) {
-				// If no mag is used, align using the yaw estimator
-				_do_ekfgsf_yaw_reset = true;
+			} else if (gps_checks_passing && !_control_status.flags.yaw_align && (_params.mag_fusion_type == MAG_FUSE_TYPE_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");
+					resetVelocityToGps(_gps_sample_delayed);
+					resetHorizontalPositionToGps(_gps_sample_delayed);
+				}
 			}
 		}
 
-		processYawEstimatorResetRequest();
-		processVelPosResetRequest();
-
 	} else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)10e6)) {
 		stopGpsFusion();
 		_warning_events.flags.gps_data_stopped = true;
@@ -161,7 +168,7 @@ bool Ekf::shouldResetGpsFusion() const
 	 * with no aiding we need to do something
 	 */
 	const bool is_reset_required = hasHorizontalAidingTimedOut()
-				     || isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max);
+				       || isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max);
 
 	/* Logic controlling the reset of navigation filter yaw to the EKF-GSF estimate to recover from loss of
 	 * navigation casued by a bad yaw estimate.
@@ -173,9 +180,9 @@ bool Ekf::shouldResetGpsFusion() const
 	 * or other sensor errors.
 	 */
 	const bool is_recent_takeoff_nav_failure = _control_status.flags.in_air
-						   && isRecent(_time_last_on_ground_us, 30000000)
-						   && isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay)
-						   && (_time_last_hor_vel_fuse > _time_last_on_ground_us);
+			&& isRecent(_time_last_on_ground_us, 30000000)
+			&& isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay)
+			&& (_time_last_hor_vel_fuse > _time_last_on_ground_us);
 
 	const bool is_inflight_nav_failure = _control_status.flags.in_air
 					     && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
@@ -197,23 +204,3 @@ bool Ekf::isYawFailure() const
 
 	return fabsf(yaw_error) > math::radians(25.f);
 }
-
-void Ekf::processYawEstimatorResetRequest()
-{
-	/* The yaw reset to the EKF-GSF estimate can be requested externally at any time during flight.
-	 * The minimum time interval between resets to the EKF-GSF estimate is limited to allow the EKF-GSF time
-	 * to improve its estimate if the previous reset was not successful.
-	 */
-	if (_do_ekfgsf_yaw_reset
-	    && isTimedOut(_ekfgsf_yaw_reset_time, 5000000)) {
-		if (resetYawToEKFGSF()) {
-			_ekfgsf_yaw_reset_time = _time_last_imu;
-			_time_last_hor_pos_fuse = _time_last_imu;
-			_time_last_hor_vel_fuse = _time_last_imu;
-
-			_do_ekfgsf_yaw_reset = false;
-			_velpos_reset_request = false; // included in yaw reset
-			_ekfgsf_yaw_reset_count++;
-		}
-	}
-}

src/modules/ekf2/EKF/gps_fusion.cpp

@@ -43,17 +43,6 @@ void Ekf::fuseGpsVelPos()
 {
 	Vector3f gps_pos_obs_var;
 
-	// correct velocity for offset relative to IMU
-	const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
-	const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
-	const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
-	_gps_sample_delayed.vel -= vel_offset_earth;
-
-	// correct position and height for offset relative to IMU
-	const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
-	_gps_sample_delayed.pos -= pos_offset_earth.xy();
-	_gps_sample_delayed.hgt += pos_offset_earth(2);
-
 	const float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
 
 	if (isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
@@ -68,7 +57,7 @@ void Ekf::fuseGpsVelPos()
 		gps_pos_obs_var(0) = gps_pos_obs_var(1) = sq(math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit));
 	}
 
-	_gps_sample_delayed.sacc = fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise);
+
 
 	const float vel_var = sq(_gps_sample_delayed.sacc);
 	const Vector3f gps_vel_obs_var{vel_var, vel_var, vel_var * sq(1.5f)};

src/modules/ekf2/EKF/mag_control.cpp

@@ -130,7 +130,6 @@ void Ekf::controlMagFusion()
 		if (_control_status.flags.in_air) {
 			checkHaglYawResetReq();
 			runInAirYawReset(mag_sample.mag);
-			runVelPosReset(); // TODO: review this; a vel/pos reset can be requested from COG reset (for fixedwing) only
 
 		} else {
 			runOnGroundYawReset();
@@ -197,7 +196,7 @@ void Ekf::runInAirYawReset(const Vector3f &mag_sample)
 	if (_mag_yaw_reset_req && isYawResetAuthorized()) {
 		bool has_realigned_yaw = false;
 
-		if (canRealignYawUsingGps()) {
+		if (_control_status.flags.gps && _control_status.flags.fixed_wing) {
 			has_realigned_yaw = realignYawGPS(mag_sample);
 
 		} else if (canResetMagHeading()) {
@@ -221,15 +220,6 @@ void Ekf::runInAirYawReset(const Vector3f &mag_sample)
 	}
 }
 
-void Ekf::runVelPosReset()
-{
-	if (_velpos_reset_request) {
-		resetVelocity();
-		resetHorizontalPosition();
-		_velpos_reset_request = false;
-	}
-}
-
 void Ekf::selectMagAuto()
 {
 	check3DMagFusionSuitability();