From a2b24fa960a14b5c7c8f646354138cdfeb9ef289 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 3 Nov 2017 12:11:32 +1100 Subject: [PATCH] EKF: Protect against ground effect induced static pressure rise during landing and takeoff. Apply a dead-zone to the vertical position innovation if using baro for height and if in the ground effect region during and just after takeoff. Method needs to be activated externally. Turns off automatically after 10 seconds or if specified height gained. --- EKF/common.h | 8 +++++++- EKF/control.cpp | 19 +++++++++++++------ EKF/estimator_interface.h | 10 ++++++++++ EKF/vel_pos_fusion.cpp | 14 ++++++++++++++ 4 files changed, 44 insertions(+), 7 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index c6fba33e67..33c6adf21b 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -192,6 +192,9 @@ struct dragSample { #define BADACC_PROBATION (uint64_t)10e6 ///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec) #define BADACC_BIAS_PNOISE 4.9f ///< The delta velocity process noise is set to this when accel data is declared bad (m/sec**2) +// ground effect compensation +#define GNDEFFECT_TIMEOUT 10E6 ///< Maximum period of time that ground effect protection will be active after it was last turned on (uSec) + struct parameters { // measurement source control int32_t fusion_mode{MASK_USE_GPS}; ///< bitmasked integer that selects which aiding sources will be used @@ -235,6 +238,8 @@ struct parameters { float baro_innov_gate{5.0f}; ///< barometric and GPS height innovation consistency gate size (STD) float posNE_innov_gate{5.0f}; ///< GPS horizontal position innovation consistency gate size (STD) float vel_innov_gate{5.0f}; ///< GPS velocity innovation consistency gate size (STD) + float gnd_effect_deadzone{5.0f}; ///< Size of deadzone applied to negative baro innovations when ground effect compensation is active (m) + float gnd_effect_max_hgt{0.5f}; ///< Height above ground at which baro ground effect becomes insignificant (m) // magnetometer fusion float mag_heading_noise{3.0e-1f}; ///< measurement noise used for simple heading fusion (rad) @@ -416,7 +421,8 @@ union filter_control_status_u { uint32_t update_mag_states_only : 1; ///< 16 - true when only the magnetometer states are updated by the magnetometer uint32_t fixed_wing : 1; ///< 17 - true when the vehicle is operating as a fixed wing vehicle uint32_t mag_fault : 1; ///< 18 - true when the magnetomer has been declared faulty and is no longer being used - uint32_t fuse_aspd : 1; ///< 19 - true when airspedd measurements are being fused + uint32_t fuse_aspd : 1; ///< 19 - true when airspeed measurements are being fused + uint32_t gnd_effect : 1; ///< 20 - true when protection from ground effect induced static pressure rise is active } flags; uint32_t value; }; diff --git a/EKF/control.cpp b/EKF/control.cpp index 1ef41f3969..55fb0a8a55 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -828,6 +828,17 @@ void Ekf::controlHeightFusion() if (_control_status_prev.flags.baro_hgt != _control_status.flags.baro_hgt) { _hgt_sensor_offset = 0.0f; } + + // Turn off ground effect compensation if it times out or sufficient height has been gained + // since takeoff. + if (_control_status.flags.gnd_effect) { + if ((_time_last_imu - _time_last_gnd_effect_on > GNDEFFECT_TIMEOUT) || + (((_last_on_ground_posD - _state.pos(2)) > _params.gnd_effect_max_hgt) && + _control_status.flags.in_air)) { + _control_status.flags.gnd_effect = false; + } + } + } else if (_control_status.flags.gps_hgt && _gps_data_ready && !_gps_hgt_faulty) { // switch to gps if there was a reset to gps _fuse_height = true; @@ -1124,11 +1135,6 @@ void Ekf::controlDragFusion() void Ekf::controlMagFusion() { - // If we are using external vision data for heading then no magnetometer fusion is used - if (_control_status.flags.ev_yaw) { - return; - } - // If we are on ground, store the local position and time to use as a reference // Also reset the flight alignment flag so that the mag fields will be re-initialised next time we achieve flight altitude if (!_control_status.flags.in_air) { @@ -1138,7 +1144,8 @@ void Ekf::controlMagFusion() } // check for new magnetometer data that has fallen behind the fusion time horizon - if (_mag_data_ready) { + // If we are using external vision data for heading then no magnetometer fusion is used + if (!_control_status.flags.ev_yaw && _mag_data_ready) { // Determine if we should use simple magnetic heading fusion which works better when there are large external disturbances // or the more accurate 3-axis fusion diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 5f9fb91c76..4312895b84 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -208,6 +208,15 @@ public: // set flag if synthetic sideslip measurement should be fused void set_fuse_beta_flag(bool fuse_beta) {_control_status.flags.fuse_beta = (fuse_beta && _control_status.flags.in_air);} + // set flag if static pressure rise due to ground effect is expected + // use _params.gnd_effect_deadzone to adjust for expected rise in static pressure + // flag will clear after GNDEFFECT_TIMEOUT uSec + void set_gnd_effect_flag(bool gnd_effect) + { + _control_status.flags.gnd_effect = gnd_effect; + _time_last_gnd_effect_on = _time_last_imu; + } + // set flag if only only mag states should be updated by the magnetometer void set_update_mag_states_only_flag(bool update_mag_states_only) {_control_status.flags.update_mag_states_only = update_mag_states_only;} @@ -473,6 +482,7 @@ protected: uint64_t _time_last_airspeed{0}; // timestamp of last airspeed measurement in microseconds uint64_t _time_last_ext_vision{0}; // timestamp of last external vision measurement in microseconds uint64_t _time_last_optflow{0}; + uint64_t _time_last_gnd_effect_on{0}; //last time the baro ground effect compensation was turned on externally (uSec) fault_status_u _fault_status{}; diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index d91ca01c3c..8db642591b 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -102,6 +102,20 @@ void Ekf::fuseVelPosHeight() // innovation gate size gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f); + // Compensate for positive static pressure transients (negative vertical position innovations) + // casued by rotor wash ground interaction by applying a temporary deadzone to baro innovations. + float deadzone_start = 0.25f * _params.baro_noise; + float deadzone_end = deadzone_start + _params.gnd_effect_deadzone; + if (_control_status.flags.gnd_effect) { + if (_vel_pos_innov[5] < -deadzone_start) { + if (_vel_pos_innov[5] <= -deadzone_end) { + _vel_pos_innov[5] += deadzone_end; + } else { + _vel_pos_innov[5] = -deadzone_start; + } + } + } + } else if (_control_status.flags.gps_hgt) { fuse_map[5] = true; // vertical position innovation - gps measurement has opposite sign to earth z axis