diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp
index 4467ff9f7e..e3c40dda77 100644
--- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp
+++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp
@@ -59,34 +59,15 @@ void Ekf::collect_gps(const gnssSample &gps)
 {
 	if (_filter_initialised && !_NED_origin_initialised && _gps_checks_passed) {
 		// If we have good GPS data set the origin's WGS-84 position to the last gps fix
-		const double lat = gps.lat;
-		const double lon = gps.lon;
-
 		if (!_pos_ref.isInitialized()) {
-			_pos_ref.initReference(lat, lon, gps.time_us);
-
-			// if we are already doing aiding, correct for the change in position since the EKF started navigating
-			if (isHorizontalAidingActive()) {
-				double est_lat;
-				double est_lon;
-				_pos_ref.reproject(-_state.pos(0), -_state.pos(1), est_lat, est_lon);
-				_pos_ref.initReference(est_lat, est_lon, gps.time_us);
-			}
+			setLatLonOriginFromCurrentPos(gps.lat, gps.lon, gps.hacc);
 		}
 
 		// Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin
 		if (!PX4_ISFINITE(_gps_alt_ref)) {
-			_gps_alt_ref = gps.alt + _state.pos(2);
+			setAltOriginFromCurrentPos(gps.alt, gps.vacc);
 		}
 
-		_NED_origin_initialised = true;
-
-		// save the horizontal and vertical position uncertainty of the origin
-		_gpos_origin_eph = gps.hacc;
-		_gpos_origin_epv = gps.vacc;
-
-		_earth_rate_NED = calcEarthRateNED(math::radians(static_cast<float>(gps.lat)));
-
 		_information_events.flags.gps_checks_passed = true;
 
 		ECL_INFO("GPS origin set to lat=%.6f, lon=%.6f",
diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h
index 44f635573c..5f2ba8c95f 100644
--- a/src/modules/ekf2/EKF/ekf.h
+++ b/src/modules/ekf2/EKF/ekf.h
@@ -261,9 +261,11 @@ public:
 	// get the ekf WGS-84 origin position and height and the system time it was last set
 	// return true if the origin is valid
 	bool getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const;
-	bool checkLatLonValidity(double latitude, double longitude, float eph = 0.f);
-	bool checkAltitudeValidity(float altitude, float epv = 0.f);
-	bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float eph = 0.f, float epv = 0.f);
+	bool checkLatLonValidity(double latitude, double longitude);
+	bool checkAltitudeValidity(float altitude);
+	bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float eph = NAN, float epv = NAN);
+	bool setEkfGlobalOriginFromCurrentPos(double latitude, double longitude, float altitude, float eph = NAN,
+					      float epv = NAN);
 
 	// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
 	void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const;
@@ -770,6 +772,9 @@ private:
 	bool setLatLonOrigin(double latitude, double longitude, float eph = NAN);
 	bool setAltOrigin(float altitude, float epv = NAN);
 
+	bool setLatLonOriginFromCurrentPos(double latitude, double longitude, float eph = NAN);
+	bool setAltOriginFromCurrentPos(float altitude, float epv = NAN);
+
 	// update quaternion states and covariances using an innovation, observation variance and Jacobian vector
 	bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW);
 	void computeYawInnovVarAndH(float variance, float &innovation_variance, VectorState &H_YAW) const;
diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp
index 4ba346e1f9..6e3bdf3bab 100644
--- a/src/modules/ekf2/EKF/ekf_helper.cpp
+++ b/src/modules/ekf2/EKF/ekf_helper.cpp
@@ -72,22 +72,18 @@ bool Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &lo
 	return _NED_origin_initialised;
 }
 
-bool Ekf::checkLatLonValidity(const double latitude, const double longitude, const float eph)
+bool Ekf::checkLatLonValidity(const double latitude, const double longitude)
 {
 	const bool lat_valid = (PX4_ISFINITE(latitude) && (abs(latitude) <= 90));
 	const bool lon_valid = (PX4_ISFINITE(longitude) && (abs(longitude) <= 180));
-	const bool eph_valid = (PX4_ISFINITE(eph) && (eph >= 0.f));
 
-	return (lat_valid && lon_valid && eph_valid);
+	return (lat_valid && lon_valid);
 }
 
-bool Ekf::checkAltitudeValidity(const float altitude, const float epv)
+bool Ekf::checkAltitudeValidity(const float altitude)
 {
 	// sanity check valid altitude anywhere between the Mariana Trench and edge of Space
-	const bool alt_valid = (PX4_ISFINITE(altitude) && ((altitude > -12'000.f) && (altitude < 100'000.f)));
-	const bool epv_valid = (PX4_ISFINITE(epv) && (epv >= 0.f));
-
-	return alt_valid && epv_valid;
+	return (PX4_ISFINITE(altitude) && ((altitude > -12'000.f) && (altitude < 100'000.f)));
 }
 
 bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude, const float eph,
@@ -105,7 +101,7 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
 
 bool Ekf::setLatLonOrigin(const double latitude, const double longitude, const float eph)
 {
-	if (!checkLatLonValidity(latitude, longitude, eph)) {
+	if (!checkLatLonValidity(latitude, longitude)) {
 		return false;
 	}
 
@@ -121,7 +117,10 @@ bool Ekf::setLatLonOrigin(const double latitude, const double longitude, const f
 
 	// reinitialize map projection to latitude, longitude, altitude, and reset position
 	_pos_ref.initReference(latitude, longitude, _time_delayed_us);
-	_gpos_origin_eph = eph;
+
+	if (PX4_ISFINITE(eph) && (eph >= 0.f)) {
+		_gpos_origin_eph = eph;
+	}
 
 	_NED_origin_initialised = true;
 
@@ -136,13 +135,16 @@ bool Ekf::setLatLonOrigin(const double latitude, const double longitude, const f
 
 bool Ekf::setAltOrigin(const float altitude, const float epv)
 {
-	if (!checkAltitudeValidity(altitude, epv)) {
+	if (!checkAltitudeValidity(altitude)) {
 		return false;
 	}
 
 	const float gps_alt_ref_prev = _gps_alt_ref;
 	_gps_alt_ref = altitude;
-	_gpos_origin_epv = epv;
+
+	if (PX4_ISFINITE(epv) && (epv >= 0.f)) {
+		_gpos_origin_epv = epv;
+	}
 
 	if (PX4_ISFINITE(gps_alt_ref_prev) && isVerticalPositionAidingActive()) {
 		// determine current z
@@ -163,6 +165,59 @@ bool Ekf::setAltOrigin(const float altitude, const float epv)
 	return true;
 }
 
+bool Ekf::setEkfGlobalOriginFromCurrentPos(const double latitude, const double longitude, const float altitude,
+		const float eph, const float epv)
+{
+	if (!setLatLonOriginFromCurrentPos(latitude, longitude, eph)) {
+		return false;
+	}
+
+	// altitude is optional
+	setAltOriginFromCurrentPos(altitude, epv);
+
+	return true;
+}
+
+bool Ekf::setLatLonOriginFromCurrentPos(const double latitude, const double longitude, const float eph)
+{
+	if (!checkLatLonValidity(latitude, longitude)) {
+		return false;
+	}
+
+	_pos_ref.initReference(latitude, longitude, _time_delayed_us);
+
+	// if we are already doing aiding, correct for the change in position since the EKF started navigating
+	if (isHorizontalAidingActive()) {
+		double est_lat;
+		double est_lon;
+		_pos_ref.reproject(-_state.pos(0), -_state.pos(1), est_lat, est_lon);
+		_pos_ref.initReference(est_lat, est_lon, _time_delayed_us);
+	}
+
+	if (PX4_ISFINITE(eph) && (eph >= 0.f)) {
+		_gpos_origin_eph = eph;
+	}
+
+	_NED_origin_initialised = true;
+
+	return true;
+}
+
+bool Ekf::setAltOriginFromCurrentPos(const float altitude, const float epv)
+{
+	if (!checkAltitudeValidity(altitude)) {
+		return false;
+	}
+
+	_gps_alt_ref = altitude + _state.pos(2);
+
+	if (PX4_ISFINITE(epv) && (epv >= 0.f)) {
+		_gpos_origin_epv = epv;
+	}
+
+	return true;
+}
+
 void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const
 {
 	float eph = INFINITY;