fix(ekf2): reject GNSS samples with vel above EKF2_VEL_LIM in EKF instead of in the on ground GNSS checks (#28663)

* fix(ekf2): reject GNSS samples with vel above EKF2_VEL_LIM in EKF instead of in the on ground GNSS checks

* fix(ekf2): drop the GNSS yaw coupling from the velocity limit skip path

GNSS yaw fusion has had its own buffer and stopped-data timeout since the heading moved to its own topic, and stopGnssFusion() no longer clears gnss_yaw. Keeping gnss_yaw in using_gnss left it true after the first stop, so stopGnssFusion() and the EKF-GSF reset re-fired on every skipped sample while yaw fusion was active, and the yaw-only timeout test could no longer pass.

Assisted-by: Claude:claude-fable-5-1

* fix(ekf2): keep the GNSS checks running while over-limit samples are skipped

Short-circuiting the checks on an over-limit sample froze the published fail flags and checks_passed at the last evaluated sample for the whole timeout window, and the eventual stop was reported as poor quality. The checks now run on every sample so their status stays truthful, the velocity limit is its own skip reason with its own stop message, and the skipped samples are counted in an EKF2 perf counter so the ulog shows why nothing was fused until estimator_status gains a fusion-state flag.

Assisted-by: Claude:claude-fable-5-1

* fix(ekf2): apply EKF2_VEL_LIM per axis to GNSS velocity samples

constrainStates() clamps each velocity component to EKF2_VEL_LIM, so the state can hold a horizontal speed up to sqrt(2) times the limit. Testing the horizontal norm rejected samples the filter could represent, and a vehicle between 100 and 141 m/s ground speed lost GNSS after the timeout. The gate now uses the same per-axis test as the clamp, and the parameter description says that samples beyond it are rejected.

Assisted-by: Claude:claude-fable-5-1

* fix(ekf2): stop GNSS height fusion when the checks time out

Height control only evaluates its fusion timeout under _gps_data_ready and its no-data branch keys on the buffer push time, which keeps advancing while samples are skipped. With HPOS and VEL disabled, a sustained check failure or over-limit velocity therefore left gps_hgt latched with nothing fused and the height reference never released. Including gps_hgt in the skip-path timeout stops it with the other GNSS aiding.

Assisted-by: Claude:claude-fable-5-1

* fix(ekf2): drop the parameter name from the velocity limit stop message

No other ECL message names a parameter.

Assisted-by: Claude:claude-fable-5-1

* refactor(ekf2): drop the velocity limit skip counter

The skipped sample still reaches updateGnssVel(), so its velocity is logged in estimator_aid_src_gnss_vel.observation with fused false while the check flags stay clear, which already names the cause. The count-delta between the EKF library and the module was scaffolding for a rare case, and a fusion-state flag in estimator_status is the planned indication. The tests observe the behaviour instead: a skipped sample stops the fusion after the timeout, an accepted one at the limit resets to it and continues.

Assisted-by: Claude:claude-fable-5-1

---------

Co-authored-by: jonas <jonas.perolini@rigi.tech>
Co-authored-by: Jacob Dahl <dahl.jakejacob@gmail.com>

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

@@ -187,16 +187,6 @@ bool GnssChecks::runInitialFixChecks(const gnssSample &gnss)
 
 	runOnGroundGnssChecks(gnss);
 
-	// force horizontal speed failure if above the limit
-	if (gnss.vel.xy().longerThan(_params.ekf2_vel_lim)) {
-		setFail(estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR, true);
-	}
-
-	// force vertical speed failure if above the limit
-	if (fabsf(gnss.vel(2)) > _params.ekf2_vel_lim) {
-		setFail(estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_SPD_ERR, true);
-	}
-
 	return enabledChecksPass(UINT16_MAX);
 }
 

src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.hpp

@@ -45,9 +45,9 @@ class GnssChecks final
 {
 public:
 	GnssChecks(int32_t &check_mask, int32_t &ekf2_req_nsats, float &ekf2_req_pdop, float &ekf2_req_eph, float &ekf2_req_epv,
-		   float &ekf2_req_sacc, float &ekf2_req_hdrift, float &ekf2_req_vdrift, int32_t &ekf2_req_fix, float &ekf2_vel_lim,
+		   float &ekf2_req_sacc, float &ekf2_req_hdrift, float &ekf2_req_vdrift, int32_t &ekf2_req_fix,
 		   uint32_t &min_health_time_us, filter_control_status_u &control_status):
-		_params{check_mask, ekf2_req_nsats, ekf2_req_pdop, ekf2_req_eph, ekf2_req_epv, ekf2_req_sacc, ekf2_req_hdrift, ekf2_req_vdrift, ekf2_req_fix, ekf2_vel_lim, min_health_time_us},
+		_params{check_mask, ekf2_req_nsats, ekf2_req_pdop, ekf2_req_eph, ekf2_req_epv, ekf2_req_sacc, ekf2_req_hdrift, ekf2_req_vdrift, ekf2_req_fix, min_health_time_us},
 		_control_status(control_status)
 	{};
 
@@ -179,7 +179,6 @@ private:
 		const float &ekf2_req_hdrift;
 		const float &ekf2_req_vdrift;
 		const int32_t &ekf2_req_fix;
-		const float &ekf2_vel_lim;
 		const uint32_t &min_health_time_us;
 	};
 

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

@@ -74,25 +74,37 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 		const gnssSample &gnss_sample = _gps_sample_delayed;
 
 		const bool initial_checks_passed_prev = _gnss_checks.initialChecksPassed();
+		const bool checks_passed = _gnss_checks.run(gnss_sample, _time_delayed_us);
 
-		if (_gnss_checks.run(gnss_sample, _time_delayed_us)) {
+		if (checks_passed && _gnss_checks.initialChecksPassed() && !initial_checks_passed_prev) {
+			// First time checks are passing, latching.
+			_information_events.flags.gps_checks_passed = true;
+		}
+
+		// Each axis of the velocity state is constrained to EKF2_VEL_LIM, so a sample beyond it cannot be fused
+		const bool vel_within_limit = gnss_sample.vel.isAllFinite()
+					      && (gnss_sample.vel.abs().max() <= _params.ekf2_vel_lim);
+
+		if (checks_passed && vel_within_limit) {
 			_time_last_gnss_checks_pass_us = _time_delayed_us;
 
-			if (_gnss_checks.initialChecksPassed() && !initial_checks_passed_prev) {
-				// First time checks are passing, latching.
-				_information_events.flags.gps_checks_passed = true;
-			}
-
 		} else {
 			// Skip this sample
 			_gps_data_ready = false;
 
-			const bool using_gnss = _control_status.flags.gnss_vel || _control_status.flags.gnss_pos;
+			const bool using_gnss = _control_status.flags.gnss_vel || _control_status.flags.gnss_pos
+						|| _control_status.flags.gps_hgt;
 			const bool gnss_checks_pass_timeout = isTimedOut(_time_last_gnss_checks_pass_us, _params.reset_timeout_max);
 
 			if (using_gnss && gnss_checks_pass_timeout) {
 				stopGnssFusion();
-				ECL_WARN("GNSS quality poor - stopping use");
+
+				if (checks_passed) {
+					ECL_WARN("GNSS velocity above limit - stopping use");
+
+				} else {
+					ECL_WARN("GNSS quality poor - stopping use");
+				}
 			}
 		}
 

src/modules/ekf2/EKF/ekf.h

@@ -615,7 +615,7 @@ private:
 	// height sensor status
 	bool _gps_intermittent{true};           ///< true if data into the buffer is intermittent
 
-	uint64_t _time_last_gnss_checks_pass_us{0}; ///< last delayed-horizon time a GNSS sample passed the checks (us)
+	uint64_t _time_last_gnss_checks_pass_us{0}; ///< last delayed-horizon time a GNSS sample passed the checks and the velocity limit (us)
 	uint64_t _time_last_gnss_fusion_stop_us{0}; ///< when GNSS velocity and position fusion were last both stopped
 
 	HeightBiasEstimator _gps_hgt_b_est{HeightSensor::GNSS, _height_sensor_ref};

src/modules/ekf2/EKF/estimator_interface.h

@@ -421,7 +421,6 @@ protected:
 			   _params.ekf2_req_hdrift,
 			   _params.ekf2_req_vdrift,
 			   _params.ekf2_req_fix,
-			   _params.ekf2_vel_lim,
 			   _min_gps_health_time_us,
 			   _control_status};
 

src/modules/ekf2/module.yaml

@@ -180,6 +180,7 @@ parameters:
     EKF2_VEL_LIM:
       description:
         short: Velocity limit
+        long: Each axis of the velocity state is constrained to this magnitude. GNSS and external vision velocity samples beyond it are not fused.
       type: float
       default: 100
       max: 299792458

src/modules/ekf2/test/test_EKF_gps.cpp

@@ -259,6 +259,28 @@ TEST_F(EkfGpsTest, gpsHgtToBaroFallback)
 	EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
 }
 
+TEST_F(EkfGpsTest, gnssHeightOnlyStopsWhenChecksFail)
+{
+	// GIVEN: GNSS height fusion is active in flight while position and velocity fusion are disabled
+	_ekf_wrapper.enableGpsHeightFusion();
+	_sensor_simulator.runSeconds(1);
+	ASSERT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion());
+
+	_ekf_wrapper.disableGpsFusion();
+	_ekf->set_in_air_status(true);
+	_ekf->set_vehicle_at_rest(false);
+	_sensor_simulator.runSeconds(2);
+	ASSERT_FALSE(_ekf_wrapper.isIntendingGpsFusion());
+	ASSERT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion());
+
+	// WHEN: the receiver fails the in-flight checks for longer than the fusion timeout
+	_sensor_simulator._gps.setFixType(2);
+	_sensor_simulator.runSeconds(8);
+
+	// THEN: height fusion stops instead of staying latched on a receiver whose samples are skipped
+	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeightFusion());
+}
+
 TEST_F(EkfGpsTest, altitudeDrift)
 {
 	// GIVEN: a drifting GNSS altitude
@@ -361,3 +383,93 @@ TEST_F(EkfGpsTest, gnssIntermittentSaccFailureDisablesFusion)
 	// and reset_timeout_max was exceeded since the last real pass.
 	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion());
 }
+
+TEST_F(EkfGpsTest, velocityAboveLimitIsNotFused)
+{
+	// GIVEN: an airborne EKF that fuses GPS with the optional quality checks disabled
+	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
+	_ekf->set_in_air_status(true);
+	_ekf->set_vehicle_at_rest(false);
+	_ekf->getParamHandle()->ekf2_gps_check = 0;
+
+	// WHEN: the receiver reports a velocity above EKF2_VEL_LIM (100 m/s by default)
+	_sensor_simulator._gps.setVelocity(Vector3f(150.f, 0.f, 0.f));
+	_sensor_simulator.runSeconds(1);
+
+	// THEN: the samples are skipped, nothing is fused while the fusion is still intended
+	const uint64_t time_last_vel_fuse = _ekf->aid_src_gnss_vel().time_last_fuse;
+	const uint64_t time_last_pos_fuse = _ekf->aid_src_gnss_pos().time_last_fuse;
+	_sensor_simulator.runSeconds(1);
+	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
+	EXPECT_EQ(_ekf->aid_src_gnss_vel().time_last_fuse, time_last_vel_fuse);
+	EXPECT_EQ(_ekf->aid_src_gnss_pos().time_last_fuse, time_last_pos_fuse);
+
+	// AND: the GNSS fusion stops once samples have been skipped for longer than the timeout
+	_sensor_simulator.runSeconds(6);
+	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion());
+
+	// AND: valid samples restart the fusion
+	_sensor_simulator._gps.setVelocity(Vector3f{});
+	_sensor_simulator.runSeconds(5);
+	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
+}
+
+TEST_F(EkfGpsTest, invalidVelocityIsSkipped)
+{
+	// GIVEN: an airborne EKF that fuses GPS with the optional quality checks disabled
+	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
+	_ekf->set_in_air_status(true);
+	_ekf->set_vehicle_at_rest(false);
+	_ekf->getParamHandle()->ekf2_gps_check = 0;
+	const float velocity_limit = _ekf->getParamHandle()->ekf2_vel_lim;
+	const Vector3f invalid_velocities[] {
+		{velocity_limit + 1.f, 0.f, 0.f},
+		{0.f, -velocity_limit - 1.f, 0.f},
+		{0.f, 0.f, velocity_limit + 1.f},
+		{0.f, 0.f, -velocity_limit - 1.f},
+		{NAN, 0.f, 0.f},
+		{0.f, INFINITY, 0.f},
+		{0.f, 0.f, -INFINITY},
+	};
+
+	for (const Vector3f &velocity : invalid_velocities) {
+		ASSERT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
+
+		// WHEN: the receiver reports an over-limit or non-finite velocity for longer than the fusion timeout
+		_sensor_simulator._gps.setVelocity(velocity);
+		_sensor_simulator.runSeconds(1); // the valid samples still in the buffer get fused
+		const uint64_t time_last_vel_fuse = _ekf->aid_src_gnss_vel().time_last_fuse;
+		const uint64_t time_last_pos_fuse = _ekf->aid_src_gnss_pos().time_last_fuse;
+		_sensor_simulator.runSeconds(7);
+
+		// THEN: nothing was fused and the fusion stopped instead of resetting to the sample
+		EXPECT_EQ(_ekf->aid_src_gnss_vel().time_last_fuse, time_last_vel_fuse);
+		EXPECT_EQ(_ekf->aid_src_gnss_pos().time_last_fuse, time_last_pos_fuse);
+		EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion());
+
+		// AND: valid samples restart it
+		_sensor_simulator._gps.setVelocity(Vector3f{});
+		_sensor_simulator.runSeconds(5);
+		EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
+	}
+}
+
+TEST_F(EkfGpsTest, velocityAtLimitIsNotSkipped)
+{
+	// GIVEN: an airborne EKF that fuses GPS
+	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
+	_ekf->set_in_air_status(true);
+	_ekf->set_vehicle_at_rest(false);
+	const float velocity_limit = _ekf->getParamHandle()->ekf2_vel_lim;
+
+	// WHEN: every component of the reported velocity sits exactly at EKF2_VEL_LIM for longer than the fusion timeout
+	_sensor_simulator._gps.setVelocity(Vector3f(velocity_limit, -velocity_limit, velocity_limit));
+	_sensor_simulator.runSeconds(1);
+	const uint64_t time_last_vel_fuse = _ekf->aid_src_gnss_vel().time_last_fuse;
+	_sensor_simulator.runSeconds(7);
+
+	// THEN: the velocity state can hold it, so the samples reach the fusion: the innovation failure resets to the
+	// sample instead of the skip timeout stopping the fusion
+	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
+	EXPECT_GT(_ekf->aid_src_gnss_vel().time_last_fuse, time_last_vel_fuse);
+}