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
PX4 · Oct 7, 2024 · 9238656 · 9238656
1 parent 4215e20
commit 9238656
Show file tree

Hide file tree

Showing 7 changed files with 26 additions and 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -487,6 +487,7 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
 		(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
@@ -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