From 5a3bc2faedcb1a8adceff31e50bd951c4b440d69 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Wed, 4 Mar 2026 15:18:34 -0900 Subject: [PATCH] fix(ekf2): apply baro ground effect deadzone symmetrically The baro ground effect deadzone was only applied to negative vertical position innovations, assuming ground effect always causes a positive static pressure transient. In practice, the direction of the pressure transient depends on the baro location on the airframe, so ground effect can produce large innovations in either direction. Apply the deadzone symmetrically to both positive and negative baro innovations. --- .../barometer/baro_height_control.cpp | 20 +++++++++---------- src/modules/ekf2/EKF/common.h | 4 ++-- src/modules/ekf2/params_barometer.yaml | 6 +++--- 3 files changed, 15 insertions(+), 15 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp index b7b45d5ced..e8cbe0e213 100644 --- a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp @@ -85,20 +85,20 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) measurement_var + bias_est.getBiasVar(), // observation variance math::max(_params.ekf2_baro_gate, 1.f)); // innovation gate - // Compensate for positive static pressure transients (negative vertical position innovations) - // caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations. + // Compensate for static pressure transients caused by rotor wash ground interaction + // by applying a temporary deadzone to baro innovations. + // The direction of the pressure transient depends on the baro location on the airframe, + // so the deadzone is applied symmetrically to both positive and negative innovations. if (_control_status.flags.gnd_effect && (_params.ekf2_gnd_eff_dz > 0.f)) { - const float deadzone_start = 0.0f; - const float deadzone_end = deadzone_start + _params.ekf2_gnd_eff_dz; + if (aid_src.innovation > _params.ekf2_gnd_eff_dz) { + aid_src.innovation -= _params.ekf2_gnd_eff_dz; - if (aid_src.innovation < -deadzone_start) { - if (aid_src.innovation <= -deadzone_end) { - aid_src.innovation += deadzone_end; + } else if (aid_src.innovation < -_params.ekf2_gnd_eff_dz) { + aid_src.innovation += _params.ekf2_gnd_eff_dz; - } else { - aid_src.innovation = -deadzone_start; - } + } else { + aid_src.innovation = 0.f; } } diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 982cb4c1af..f60f00cd7b 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -311,7 +311,7 @@ struct parameters { float baro_bias_nsd{0.13f}; ///< process noise for barometric height bias estimation (m/s/sqrt(Hz)) float ekf2_baro_gate{5.0f}; ///< barometric and GPS height innovation consistency gate size (STD) - float ekf2_gnd_eff_dz{5.0f}; ///< Size of deadzone applied to negative baro innovations when ground effect compensation is active (m) + float ekf2_gnd_eff_dz{5.0f}; ///< Size of deadzone applied to baro innovations when ground effect compensation is active (m) float ekf2_gnd_max_hgt{0.5f}; ///< Height above ground at which baro ground effect becomes insignificant (m) # if defined(CONFIG_EKF2_BARO_COMPENSATION) @@ -553,7 +553,7 @@ uint64_t mag_fault : 1; ///< 18 - true when the magnetometer has been declared faulty and is no longer being used uint64_t fuse_aspd : 1; ///< 19 - true when airspeed measurements are being fused uint64_t gnd_effect : - 1; ///< 20 - true when protection from ground effect induced static pressure rise is active + 1; ///< 20 - true when protection from ground effect induced static pressure transients is active uint64_t rng_stuck : 1; ///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough uint64_t gnss_yaw : diff --git a/src/modules/ekf2/params_barometer.yaml b/src/modules/ekf2/params_barometer.yaml index cefa503039..85a4c25049 100644 --- a/src/modules/ekf2/params_barometer.yaml +++ b/src/modules/ekf2/params_barometer.yaml @@ -42,7 +42,7 @@ parameters: EKF2_GND_EFF_DZ: description: short: Baro deadzone range for height fusion - long: Sets the value of deadzone applied to negative baro innovations. Deadzone + long: Sets the value of deadzone applied to baro innovations. Deadzone is enabled when EKF2_GND_EFF_DZ > 0. type: float default: 4.0 @@ -53,8 +53,8 @@ parameters: EKF2_GND_MAX_HGT: description: short: Height above ground level for ground effect zone - long: Sets the maximum distance to the ground level where negative baro innovations - are expected. + long: Sets the maximum distance to the ground level where baro ground effect + is expected. type: float default: 0.5 min: 0.0