ekf2: split horizontal and vertical origin reset

Allow partial resets (only lat/lon or only altitude)

src/modules/ekf2/EKF/ekf.h

@@ -261,6 +261,8 @@ 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);
 
 	// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
@@ -765,6 +767,9 @@ private:
 		P.slice<S.dof, S.dof>(S.idx, S.idx) = cov;
 	}
 
+	bool setLatLonOrigin(double latitude, double longitude, float eph = NAN);
+	bool setAltOrigin(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;

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -72,63 +72,95 @@ 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)
+{
+	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);
+}
+
+bool Ekf::checkAltitudeValidity(const float altitude, const float epv)
+{
+	// 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;
+}
+
 bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude, const float eph,
 			     const float epv)
 {
-	// sanity check valid latitude/longitude and altitude anywhere between the Mariana Trench and edge of Space
+	if (!setLatLonOrigin(latitude, longitude, eph)) {
-	if (PX4_ISFINITE(latitude) && (abs(latitude) <= 90)
+		return false;
-	    && PX4_ISFINITE(longitude) && (abs(longitude) <= 180)
-	    && PX4_ISFINITE(altitude) && (altitude > -12'000.f) && (altitude < 100'000.f)
-	   ) {
-		bool current_pos_available = false;
-		double current_lat = static_cast<double>(NAN);
-		double current_lon = static_cast<double>(NAN);
-
-		// if we are already doing aiding, correct for the change in position since the EKF started navigating
-		if (_pos_ref.isInitialized() && isHorizontalAidingActive()) {
-			_pos_ref.reproject(_state.pos(0), _state.pos(1), current_lat, current_lon);
-			current_pos_available = true;
-		}
-
-		const float gps_alt_ref_prev = _gps_alt_ref;
-
-		// reinitialize map projection to latitude, longitude, altitude, and reset position
-		_pos_ref.initReference(latitude, longitude, _time_delayed_us);
-		_gps_alt_ref = altitude;
-
-		_gpos_origin_eph = eph;
-		_gpos_origin_epv = epv;
-
-		_NED_origin_initialised = true;
-
-		_earth_rate_NED = calcEarthRateNED(math::radians(static_cast<float>(latitude)));
-
-		if (current_pos_available) {
-			// reset horizontal position if we already have a global origin
-			Vector2f position = _pos_ref.project(current_lat, current_lon);
-			resetHorizontalPositionTo(position);
-		}
-
-		if (PX4_ISFINITE(gps_alt_ref_prev) && isVerticalPositionAidingActive()) {
-			// determine current z
-			const float z_prev = _state.pos(2);
-			const float current_alt = -z_prev + gps_alt_ref_prev;
-#if defined(CONFIG_EKF2_GNSS)
-			const float gps_hgt_bias = _gps_hgt_b_est.getBias();
-#endif // CONFIG_EKF2_GNSS
-			resetVerticalPositionTo(_gps_alt_ref - current_alt);
-			ECL_DEBUG("EKF global origin updated, resetting vertical position %.1fm -> %.1fm", (double)z_prev,
-				  (double)_state.pos(2));
-#if defined(CONFIG_EKF2_GNSS)
-			// adjust existing GPS height bias
-			_gps_hgt_b_est.setBias(gps_hgt_bias);
-#endif // CONFIG_EKF2_GNSS
-		}
-
-		return true;
 	}
 
-	return false;
+	// altitude is optional
+	setAltOrigin(altitude, epv);
+
+	return true;
+}
+
+bool Ekf::setLatLonOrigin(const double latitude, const double longitude, const float eph)
+{
+	if (!checkLatLonValidity(latitude, longitude, eph)) {
+		return false;
+	}
+
+	bool current_pos_available = false;
+	double current_lat = static_cast<double>(NAN);
+	double current_lon = static_cast<double>(NAN);
+
+	// if we are already doing aiding, correct for the change in position since the EKF started navigating
+	if (_pos_ref.isInitialized() && isHorizontalAidingActive()) {
+		_pos_ref.reproject(_state.pos(0), _state.pos(1), current_lat, current_lon);
+		current_pos_available = true;
+	}
+
+	// reinitialize map projection to latitude, longitude, altitude, and reset position
+	_pos_ref.initReference(latitude, longitude, _time_delayed_us);
+	_gpos_origin_eph = eph;
+
+	_NED_origin_initialised = true;
+
+	if (current_pos_available) {
+		// reset horizontal position if we already have a global origin
+		Vector2f position = _pos_ref.project(current_lat, current_lon);
+		resetHorizontalPositionTo(position);
+	}
+
+	return true;
+}
+
+bool Ekf::setAltOrigin(const float altitude, const float epv)
+{
+	if (!checkAltitudeValidity(altitude, epv)) {
+		return false;
+	}
+
+	const float gps_alt_ref_prev = _gps_alt_ref;
+	_gps_alt_ref = altitude;
+	_gpos_origin_epv = epv;
+
+	if (PX4_ISFINITE(gps_alt_ref_prev) && isVerticalPositionAidingActive()) {
+		// determine current z
+		const float z_prev = _state.pos(2);
+		const float current_alt = -z_prev + gps_alt_ref_prev;
+#if defined(CONFIG_EKF2_GNSS)
+		const float gps_hgt_bias = _gps_hgt_b_est.getBias();
+#endif // CONFIG_EKF2_GNSS
+		resetVerticalPositionTo(_gps_alt_ref - current_alt);
+		ECL_DEBUG("EKF global origin updated, resetting vertical position %.1fm -> %.1fm", (double)z_prev,
+			  (double)_state.pos(2));
+#if defined(CONFIG_EKF2_GNSS)
+		// adjust existing GPS height bias
+		_gps_hgt_b_est.setBias(gps_hgt_bias);
+#endif // CONFIG_EKF2_GNSS
+	}
+
+	return true;
 }
 
 void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const