AP_NavEKF: Compensate for ground effect when takeoff or landing expected

libraries/AP_NavEKF/AP_NavEKF.cpp

@@ -401,7 +401,10 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) :
     DCM33FlowMin(0.71f),            // If Tbn(3,3) is less than this number, optical flow measurements will not be fused as tilt is too high.
     fScaleFactorPnoise(1e-10f),     // Process noise added to focal length scale factor state variance at each time step
     flowTimeDeltaAvg_ms(100),       // average interval between optical flow measurements (msec)
-    flowIntervalMax_ms(100)         // maximum allowable time between flow fusion events
+    flowIntervalMax_ms(100),        // maximum allowable time between flow fusion events
+    gndEffectTO_ms(30000),          // time in msec that baro ground effect compensation will timeout after initiation
+    gndEffectBaroScaler(4.0f),      // scaler applied to the barometer observation variance when operating in ground effect
+    gndEffectBaroTO_ms(5000)        // time in msec that the baro measurement will be rejected if the gndEffectBaroVarLim has failed it
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
     ,_perf_UpdateFilter(perf_alloc(PC_ELAPSED, "EKF_UpdateFilter")),
@@ -1933,6 +1936,10 @@ void NavEKF::FuseVelPosNED()
         R_OBS[3] = sq(constrain_float(_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr);
         R_OBS[4] = R_OBS[3];
         R_OBS[5] = sq(constrain_float(_baroAltNoise, 0.1f, 10.0f));
+        // reduce weighting (increase observation noise) on baro if we are likely to be in ground effect
+        if (getGndEffectMode()) {
+            R_OBS[5] *= gndEffectBaroScaler;
+        }
 
         // if vertical GPS velocity data is being used, check to see if the GPS vertical velocity and barometer
         // innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting
@@ -4727,4 +4734,33 @@ bool NavEKF::setOriginLLH(struct Location &loc)
     return true;
 }
 
+// determine if the baro ground effect compensation is active
+bool NavEKF::getGndEffectMode(void)
+{
+    if (gndEffectMode && (imuSampleTime_ms - startTimeTO_ms < gndEffectTO_ms) && gndEffectTO_ms != 0)  {
+        gndEffectMode = true;
+    } else {
+        gndEffectMode = false;
+        startTimeTO_ms = 0;
+    }
+    return gndEffectMode;
+}
+
+
+// Tell the EKF to de-weight the baro sensor to take account of ground effect on baro during takeoff of landing when set to true
+// Should be set to off by sending a false when the ground effect height region is cleared or the landing completed
+// If not reactivated, this condition times out after the number of msec set by the _gndEffectTO_ms parameter
+// The amount of de-weighting is controlled by the gndEffectBaroScaler parameter
+void NavEKF::setGndEffectMode(bool setMode)
+{
+    if  (setMode) {
+        startTimeTO_ms = imuSampleTime_ms;
+        gndEffectMode = true;
+    } else {
+        gndEffectMode = true;
+        startTimeTO_ms = 0;
+    }
+}
+
+
 #endif // HAL_CPU_CLASS

libraries/AP_NavEKF/AP_NavEKF.h

@@ -194,6 +194,12 @@ public:
     // return data for debugging optical flow fusion
     void getFlowDebug(float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const;
 
+    // Tells the EKF to de-weight the baro sensor to take account of ground effect on baro during takeoff of landing when set to true
+    // Should be set to off by sending a false when the ground effect height region is cleared or the landing completed
+    // If not reactivated, this condition times out after the number of msec set by the _gndEffectTO_ms parameter
+    // The amount of de-weighting is controlled by the gndEffectBaroScaler parameter
+    void setGndEffectMode(bool modeOn);
+
     /*
     return the filter fault status as a bitmasked integer
      0 = quaternions are NaN
@@ -405,6 +411,10 @@ private:
     // Set the NED origin to be used until the next filter reset
     void setOrigin();
 
+    // get status of baro ground effect compensation mode
+    // returns true when baro ground effect compensation is required
+    bool getGndEffectMode(void);
+
     // EKF Mavlink Tuneable Parameters
     AP_Float _gpsHorizVelNoise;     // GPS horizontal velocity measurement noise : m/s
     AP_Float _gpsVertVelNoise;      // GPS vertical velocity measurement noise : m/s
@@ -442,6 +452,9 @@ private:
     // Tuning parameters
     const float gpsNEVelVarAccScale;    // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration
     const float gpsDVelVarAccScale;     // Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration
+    const uint16_t gndEffectTO_ms;      // time in msec that ground effect mode is active after being activated
+    const float gndEffectBaroScaler;    // scaler applied to the barometer observation variance when ground effect mode is active
+    const float gndEffectBaroTO_ms;     // time in msec that the baro measurement will be rejected if the gndEffectBaroVarLim has failed it
     const float gpsPosVarAccScale;      // Scale factor applied to horizontal position measurement variance due to manoeuvre acceleration
     const float msecHgtDelay;           // Height measurement delay (msec)
     const uint16_t msecMagDelay;        // Magnetometer measurement delay (msec)
@@ -601,6 +614,9 @@ private:
     bool prevVehicleArmed;          // vehicleArmed from previous frame
     struct Location EKF_origin;     // LLH origin of the NED axis system - do not change unless filter is reset
     bool validOrigin;               // true when the EKF origin is valid
+    bool gndEffectMode;             // modifiy baro processing to compensate for ground effect
+    uint32_t startTimeTO_ms;        // time in msec that the takeoff condition started - used to compensate for ground effect on baro height
+    uint32_t startTimeLAND_ms;      // time in msec that the landing condition started - used to compensate for ground effect on baro height
     float gpsSpdAccuracy;           // estimated speed accuracy in m/s returned by the UBlox GPS receiver
 
     // Used by smoothing of state corrections