ekf2: fix manual pos reset during constant position fusion

This is not only needed when GNSS is available but also for other global
sources of position (e.g.: aux global pos and manual pos updates)

src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp

@@ -113,7 +113,8 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
 
 				} else {
 					// Try to initialize using measurement
-					if (ekf.setEkfGlobalOrigin(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph, sample.epv)) {
+					if (ekf.setEkfGlobalOriginFromCurrentPos(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph,
+							sample.epv)) {
 						ekf.enableControlStatusAuxGpos();
 						_reset_counters.lat_lon = sample.lat_lon_reset_counter;
 						_state = State::active;

src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp

@@ -63,32 +63,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;
-
 		_information_events.flags.gps_checks_passed = true;
 		ECL_INFO("GPS checks passed");
 	}

src/modules/ekf2/EKF/ekf.cpp

@@ -276,68 +276,100 @@ void Ekf::predictState(const imuSample &imu_delayed)
 	_height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf;
 }
 
-bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy,
-		uint64_t timestamp_observation)
+bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const double longitude, const float altitude,
+		const float eph,
+		const float epv, uint64_t timestamp_observation)
 {
-	if (!_pos_ref.isInitialized()) {
-		ECL_WARN("unable to reset global position, position reference not initialized");
+	if (!checkLatLonValidity(latitude, longitude)) {
 		return false;
 	}
 
-	Vector2f pos_corrected = _pos_ref.project(lat_deg, lon_deg);
+	if (!_pos_ref.isInitialized()) {
+		if (!setLatLonOriginFromCurrentPos(latitude, longitude, eph)) {
+			return false;
+		}
+
+		if (!PX4_ISFINITE(_gps_alt_ref)) {
+			setAltOriginFromCurrentPos(altitude, epv);
+		}
+
+		return true;
+	}
+
+	Vector3f pos_correction;
 
 	// apply a first order correction using velocity at the delayed time horizon and the delta time
-	if ((timestamp_observation > 0) && (isHorizontalAidingActive() || !_horizontal_deadreckon_time_exceeded)) {
+	if ((timestamp_observation > 0) && local_position_is_valid()) {
 
 		timestamp_observation = math::min(_time_latest_us, timestamp_observation);
 
-		float diff_us = 0.f;
+		float dt_us;
 
 		if (_time_delayed_us >= timestamp_observation) {
-			diff_us = static_cast<float>(_time_delayed_us - timestamp_observation);
+			dt_us = static_cast<float>(_time_delayed_us - timestamp_observation);
 
 		} else {
-			diff_us = -static_cast<float>(timestamp_observation - _time_delayed_us);
+			dt_us = -static_cast<float>(timestamp_observation - _time_delayed_us);
 		}
 
-		const float dt_s = diff_us * 1e-6f;
-		pos_corrected += _state.vel.xy() * dt_s;
+		const float dt_s = dt_us * 1e-6f;
+		pos_correction = _state.vel * dt_s;
 	}
 
-	const float obs_var = math::max(sq(accuracy), sq(0.01f));
+	{
+		const Vector2f hpos = _pos_ref.project(latitude, longitude) + pos_correction.xy();
 
-	const Vector2f innov = Vector2f(_state.pos.xy()) - pos_corrected;
-	const Vector2f innov_var = Vector2f(getStateVariance<State::pos>()) + obs_var;
+		const float obs_var = math::max(sq(eph), sq(0.01f));
 
-	const float sq_gate = sq(5.f); // magic hardcoded gate
-	const Vector2f test_ratio{sq(innov(0)) / (sq_gate * innov_var(0)),
-				  sq(innov(1)) / (sq_gate * innov_var(1))};
+		const Vector2f innov = Vector2f(_state.pos.xy()) - hpos;
+		const Vector2f innov_var = Vector2f(getStateVariance<State::pos>()) + obs_var;
 
-	const bool innov_rejected = (test_ratio.max() > 1.f);
+		const float sq_gate = sq(5.f); // magic hardcoded gate
+		const float test_ratio = sq(innov(0)) / (sq_gate * innov_var(0)) + sq(innov(1)) / (sq_gate * innov_var(1));
 
-	if (!_control_status.flags.in_air || (accuracy > 0.f && accuracy < 1.f) || innov_rejected) {
-		// when on ground or accuracy chosen to be very low, we hard reset position
-		// this allows the user to still send hard resets at any time
-		ECL_INFO("reset position to external observation");
-		_information_events.flags.reset_pos_to_ext_obs = true;
+		const bool innov_rejected = (test_ratio > 1.f);
 
-		resetHorizontalPositionTo(pos_corrected, obs_var);
-		_last_known_pos.xy() = _state.pos.xy();
-		return true;
+		if (!_control_status.flags.in_air || (eph > 0.f && eph < 1.f) || innov_rejected) {
+			// when on ground or accuracy chosen to be very low, we hard reset position
+			// this allows the user to still send hard resets at any time
+			ECL_INFO("reset position to external observation");
+			_information_events.flags.reset_pos_to_ext_obs = true;
 
-	} else {
-		if (fuseDirectStateMeasurement(innov(0), innov_var(0), obs_var, State::pos.idx + 0)
-		    && fuseDirectStateMeasurement(innov(1), innov_var(1), obs_var, State::pos.idx + 1)
-		   ) {
-			ECL_INFO("fused external observation as position measurement");
+			resetHorizontalPositionTo(hpos, obs_var);
+			_last_known_pos.xy() = _state.pos.xy();
+
+		} else {
+			ECL_INFO("fuse external observation as position measurement");
+			fuseDirectStateMeasurement(innov(0), innov_var(0), obs_var, State::pos.idx + 0);
+			fuseDirectStateMeasurement(innov(1), innov_var(1), obs_var, State::pos.idx + 1);
+
+			// Use the reset counters to inform the controllers about a potential large position jump
+			// TODO: compute the actual position change
 			_state_reset_status.reset_count.posNE++;
+			_state_reset_status.posNE_change.zero();
+
 			_time_last_hor_pos_fuse = _time_delayed_us;
 			_last_known_pos.xy() = _state.pos.xy();
-			return true;
 		}
 	}
 
-	return false;
+	if (checkAltitudeValidity(altitude)) {
+		const float altitude_corrected = altitude - pos_correction(2);
+
+		if (!PX4_ISFINITE(_gps_alt_ref)) {
+			setAltOriginFromCurrentPos(altitude_corrected, epv);
+
+		} else {
+			const float vpos = -(altitude_corrected - _gps_alt_ref);
+			const float obs_var = math::max(sq(epv), sq(0.01f));
+
+			ECL_INFO("reset height to external observation");
+			resetVerticalPositionTo(vpos, obs_var);
+			_last_known_pos(2) = _state.pos(2);
+		}
+	}
+
+	return true;
 }
 
 void Ekf::updateParameters()

src/modules/ekf2/EKF/ekf.h

@@ -258,7 +258,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 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);
 	void updateWmm(double lat, double lon);
 
 	// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
@@ -524,7 +528,7 @@ public:
 		return true;
 	}
 
-	bool resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy,
+	bool resetGlobalPosToExternalObservation(double latitude, double longitude, float altitude, float eph, float epv,
 			uint64_t timestamp_observation);
 
 	/**
@@ -758,6 +762,12 @@ 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);
+
+	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;

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -72,63 +72,152 @@ 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 bool lat_valid = (PX4_ISFINITE(latitude) && (abs(latitude) <= 90));
+	const bool lon_valid = (PX4_ISFINITE(longitude) && (abs(longitude) <= 180));
+
+	return (lat_valid && lon_valid);
+}
+
+bool Ekf::checkAltitudeValidity(const float altitude)
+{
+	// sanity check valid altitude anywhere between the Mariana Trench and edge of Space
+	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,
 			     const float epv)
 {
-	// sanity check valid latitude/longitude and altitude anywhere between the Mariana Trench and edge of Space
-	if (PX4_ISFINITE(latitude) && (abs(latitude) <= 90)
-	    && 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;
-
-		updateWmm(current_lat, current_lon);
-
-		_gpos_origin_eph = eph;
-		_gpos_origin_epv = epv;
-
-		_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);
-		}
-
-		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;
+	if (!setLatLonOrigin(latitude, longitude, eph)) {
+		return false;
 	}
 
-	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)) {
+		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() && local_position_is_valid()) {
+		_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);
+
+	if (PX4_ISFINITE(eph) && (eph >= 0.f)) {
+		_gpos_origin_eph = eph;
+	}
+
+	_NED_origin_initialised = true;
+
+	updateWmm(current_lat, current_lon);
+
+	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)) {
+		return false;
+	}
+
+	const float gps_alt_ref_prev = _gps_alt_ref;
+	_gps_alt_ref = altitude;
+
+	if (PX4_ISFINITE(epv) && (epv >= 0.f)) {
+		_gpos_origin_epv = epv;
+	}
+
+	if (PX4_ISFINITE(gps_alt_ref_prev) && isLocalVerticalPositionValid()) {
+		// 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;
+}
+
+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 (local_position_is_valid()) {
+		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::updateWmm(const double lat, const double lon)

src/modules/ekf2/EKF2.cpp

@@ -545,9 +545,8 @@ void EKF2::Run()
 						accuracy = vehicle_command.param3;
 					}
 
-					// TODO add check for lat and long validity
-					if (_ekf.resetGlobalPosToExternalObservation(vehicle_command.param5, vehicle_command.param6,
-							accuracy, timestamp_observation)
+					if (_ekf.resetGlobalPosToExternalObservation(vehicle_command.param5, vehicle_command.param6, vehicle_command.param7,
+							accuracy, accuracy, timestamp_observation)
 					   ) {
 						command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 

src/modules/ekf2/test/CMakeLists.txt

@@ -37,7 +37,7 @@ add_subdirectory(sensor_simulator)
 add_subdirectory(test_helper)
 
 px4_add_unit_gtest(SRC test_EKF_accelerometer.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
-px4_add_unit_gtest(SRC test_EKF_airspeed.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
+px4_add_unit_gtest(SRC test_EKF_airspeed.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_basics.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_externalVision.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_fake_pos.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)

src/modules/ekf2/test/test_EKF_airspeed.cpp

@@ -40,6 +40,7 @@
 #include "EKF/ekf.h"
 #include "sensor_simulator/sensor_simulator.h"
 #include "sensor_simulator/ekf_wrapper.h"
+#include "test_helper/reset_logging_checker.h"
 
 
 class EkfAirspeedTest : public ::testing::Test
@@ -182,9 +183,11 @@ TEST_F(EkfAirspeedTest, testAirspeedDeadReckoning)
 	const double latitude_new  = -15.0000005;
 	const double longitude_new = -115.0000005;
 	const float altitude_new  = 1500.0;
+	const float eph = 50.f;
+	const float epv = 10.f;
 
 	_ekf->setEkfGlobalOrigin(latitude_new, longitude_new, altitude_new);
-	_ekf->resetGlobalPosToExternalObservation(latitude_new, longitude_new, 50.f, 0);
+	_ekf->resetGlobalPosToExternalObservation(latitude_new, longitude_new, altitude_new, eph, epv, 0);
 
 	// Simulate the fact that the sideslip can start immediately, without
 	// waiting for a measurement sample.
@@ -203,4 +206,78 @@ TEST_F(EkfAirspeedTest, testAirspeedDeadReckoning)
 	const Vector2f vel_wind_earth = _ekf->getWindVelocity();
 	EXPECT_NEAR(vel_wind_earth(0), 0.f, .1f);
 	EXPECT_NEAR(vel_wind_earth(1), 0.f, .1f);
+
+	EXPECT_TRUE(_ekf->global_position_is_valid());
+}
+
+TEST_F(EkfAirspeedTest, testAirspeedDeadReckoningLatLonAltReset)
+{
+	// GIVEN: a flying fixed-wing dead-reckoning with airspeed and sideslip fusion
+	const Vector3f simulated_velocity_earth(-3.6f, 8.f, 0.0f);
+	const Vector2f airspeed_body(15.f, 0.0f);
+	_sensor_simulator.runSeconds(10);
+
+	_ekf->set_in_air_status(true);
+	_ekf->set_vehicle_at_rest(false);
+	_ekf->set_is_fixed_wing(true);
+
+	double latitude  = -15.0000005;
+	double longitude = -115.0000005;
+	float altitude  = 1500.0;
+	const float eph = 50.f;
+	const float epv = 1.f;
+
+	_ekf->setEkfGlobalOrigin(latitude, longitude, altitude);
+	_ekf->resetGlobalPosToExternalObservation(latitude, longitude, altitude, eph, epv, 0);
+
+	_ekf_wrapper.enableBetaFusion();
+	_sensor_simulator.runSeconds(1.f);
+	EXPECT_TRUE(_ekf_wrapper.isIntendingBetaFusion());
+
+	_sensor_simulator.startAirspeedSensor();
+	_sensor_simulator._airspeed.setData(airspeed_body(0), airspeed_body(0));
+	_sensor_simulator.runSeconds(10.f);
+	EXPECT_TRUE(_ekf_wrapper.isIntendingAirspeedFusion());
+
+	EXPECT_TRUE(_ekf->global_position_is_valid());
+
+	// WHEN: an external position reset is sent
+	ResetLoggingChecker reset_logging_checker(_ekf);
+	reset_logging_checker.capturePreResetState();
+
+	double latitude_new = -16.0000005;
+	double longitude_new = -116.0000005;
+	float altitude_new = 1602.0;
+	_ekf->resetGlobalPosToExternalObservation(latitude_new, longitude_new, altitude_new, eph, epv, 0);
+
+	const Vector3f pos_new = _ekf->getPosition();
+	const float altitude_est = -pos_new(2) + _ekf->getEkfGlobalOriginAltitude();
+
+	double latitude_est, longitude_est;
+	_ekf->global_origin().reproject(pos_new(0), pos_new(1), latitude_est, longitude_est);
+
+	// THEN: the global position is adjusted accordingly
+	EXPECT_NEAR(altitude_est, altitude_new, 0.01f);
+	EXPECT_NEAR(latitude_est, latitude_new, 1e-3f);
+	EXPECT_NEAR(longitude_est, longitude_new, 1e-3f);
+	EXPECT_TRUE(_ekf->global_position_is_valid());
+
+	reset_logging_checker.capturePostResetState();
+	EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(0));
+	EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1));
+	EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(0));
+	EXPECT_TRUE(reset_logging_checker.isHorizontalPositionResetCounterIncreasedBy(1));
+
+	// AND WHEN: only the lat/lon is valid
+	latitude_new = -16.0000005;
+	longitude_new = -116.0000005;
+	altitude_new = NAN;
+	_ekf->resetGlobalPosToExternalObservation(latitude_new, longitude_new, altitude_new, eph, epv, 0);
+
+	// THEN: lat/lon are reset but not the altitude
+	reset_logging_checker.capturePostResetState();
+	EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(0));
+	EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1));
+	EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(0));
+	EXPECT_TRUE(reset_logging_checker.isHorizontalPositionResetCounterIncreasedBy(2));
 }