From 9238656a3385eb67c054460f3786b568149e0c95 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 4 Oct 2024 13:25:30 -0400
Subject: [PATCH] ekf2: configurable velocity state constraint (EKF2_VEL_LIM)

 - replace hardcoded 1000 m/s velocity state constraint with new EKF2_VEL_LIM parameter (default 100 m/s)
 - new velocity limit also used for GPS checks and external vision velocity
---
 .../EKF/aid_sources/external_vision/ev_vel_control.cpp |  3 ++-
 src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp   | 10 ++++++++++
 src/modules/ekf2/EKF/common.h                          |  2 ++
 src/modules/ekf2/EKF/ekf.cpp                           |  2 +-
 src/modules/ekf2/EKF2.cpp                              |  1 +
 src/modules/ekf2/EKF2.hpp                              |  1 +
 src/modules/ekf2/module.yaml                           |  9 +++++++++
 7 files changed, 26 insertions(+), 2 deletions(-)

diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp
index ac2ff78f24..c399a44adb 100644
--- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp
+++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp
@@ -53,7 +53,8 @@ void Ekf::controlEvVelFusion(ExternalVisionVel &ev, const bool common_starting_c
 	// determine if we should use EV velocity aiding
 	bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))
 					     && _control_status.flags.tilt_align
-					     && ev._sample.vel.isAllFinite();
+					     && ev._sample.vel.isAllFinite()
+					     && !ev._sample.vel.longerThan(_params.velocity_limit);
 
 
 	continuing_conditions_passing &= ev._measurement.isAllFinite() && ev._measurement_var.isAllFinite();
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 8d3cbd9567..236fd2dd63 100644
--- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp
+++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp
@@ -172,6 +172,16 @@ bool Ekf::runGnssChecks(const gnssSample &gps)
 		resetGpsDriftCheckFilters();
 	}
 
+	// force horizontal speed failure if above the limit
+	if (gps.vel.xy().longerThan(_params.velocity_limit)) {
+		_gps_check_fail_status.flags.hspeed = true;
+	}
+
+	// force vertical speed failure if above the limit
+	if (fabsf(gps.vel(2)) > _params.velocity_limit) {
+		_gps_check_fail_status.flags.vspeed = true;
+	}
+
 	// save GPS fix for next time
 	_gps_pos_prev.initReference(lat, lon, gps.time_us);
 	_gps_alt_prev = gps.alt;
diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h
index cc1d48cf16..6c1ef2d6f4 100644
--- a/src/modules/ekf2/EKF/common.h
+++ b/src/modules/ekf2/EKF/common.h
@@ -270,6 +270,8 @@ struct parameters {
 
 	int32_t imu_ctrl{static_cast<int32_t>(ImuCtrl::GyroBias) | static_cast<int32_t>(ImuCtrl::AccelBias)};
 
+	float velocity_limit{100.f};           ///< velocity state limit (m/s)
+
 	// measurement source control
 	int32_t height_sensor_ref{static_cast<int32_t>(HeightSensor::BARO)};
 	int32_t position_sensor_ref{static_cast<int32_t>(PositionSensor::GNSS)};
diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp
index 910eb54dd6..9b87b3d0e1 100644
--- a/src/modules/ekf2/EKF/ekf.cpp
+++ b/src/modules/ekf2/EKF/ekf.cpp
@@ -261,7 +261,7 @@ void Ekf::predictState(const imuSample &imu_delayed)
 	_state.pos += (vel_last + _state.vel) * imu_delayed.delta_vel_dt * 0.5f;
 
 	// constrain states
-	_state.vel = matrix::constrain(_state.vel, -1000.f, 1000.f);
+	_state.vel = matrix::constrain(_state.vel, -_params.velocity_limit, _params.velocity_limit);
 	_state.pos = matrix::constrain(_state.pos, -1.e6f, 1.e6f);
 
 
diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp
index f7e64ce194..20129678e4 100644
--- a/src/modules/ekf2/EKF2.cpp
+++ b/src/modules/ekf2/EKF2.cpp
@@ -66,6 +66,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_predict_us(_params->filter_update_interval_us),
 	_param_ekf2_delay_max(_params->delay_max_ms),
 	_param_ekf2_imu_ctrl(_params->imu_ctrl),
+	_param_ekf2_vel_lim(_params->velocity_limit),
 #if defined(CONFIG_EKF2_AUXVEL)
 	_param_ekf2_avel_delay(_params->auxvel_delay_ms),
 #endif // CONFIG_EKF2_AUXVEL
diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp
index 57b3cc749c..61105a3c9b 100644
--- a/src/modules/ekf2/EKF2.hpp
+++ b/src/modules/ekf2/EKF2.hpp
@@ -487,6 +487,7 @@ private:
 		(ParamExtInt<px4::params::EKF2_PREDICT_US>) _param_ekf2_predict_us,
 		(ParamExtFloat<px4::params::EKF2_DELAY_MAX>) _param_ekf2_delay_max,
 		(ParamExtInt<px4::params::EKF2_IMU_CTRL>) _param_ekf2_imu_ctrl,
+		(ParamExtFloat<px4::params::EKF2_VEL_LIM>) _param_ekf2_vel_lim,
 
 #if defined(CONFIG_EKF2_AUXVEL)
 		(ParamExtFloat<px4::params::EKF2_AVEL_DELAY>)
diff --git a/src/modules/ekf2/module.yaml b/src/modules/ekf2/module.yaml
index da68460445..d8295fd60c 100644
--- a/src/modules/ekf2/module.yaml
+++ b/src/modules/ekf2/module.yaml
@@ -172,3 +172,12 @@ parameters:
       max: 1.0
       unit: s
       decimal: 2
+
+    EKF2_VEL_LIM:
+      description:
+        short: Velocity limit
+      type: float
+      default: 100
+      max: 299792458
+      unit: m/s
+      decimal: 1