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

@@ -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;
 };

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

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{};
 

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