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
This commit is contained in:
Daniel Agar
2024-10-07 10:35:48 +02:00
committed by Mathieu Bresciani
parent 4215e20120
commit 9238656a33
7 changed files with 26 additions and 2 deletions
+1 -1
View File
@@ -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);