fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-06 21:38:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix(ekf2): apply baro ground effect deadzone symmetrically

Browse Source 
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.
This commit is contained in:
Jacob Dahl
2026-03-04 15:18:34 -09:00
parent f11329784f
commit 5a3bc2faed
3 changed files with 15 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp
+10 -10
View File
@@ -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;
}
}
src/modules/ekf2/EKF/common.h
+2 -2
View File
@@ -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 :
src/modules/ekf2/params_barometer.yaml
+3 -3
View File
@@ -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