AP_NavEKF2: Add function to LPF and spike filter baro data

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

@@ -712,6 +712,15 @@ void NavEKF2_core::readHgtData()
     }
 }
 
+// calculate filtered offset between baro height measurement and EKF height estimate
+// offset should be subtracted from baro measurement to match filter estimate
+// offset is used to enable reversion to baro if alternate height data sources fail
+void NavEKF2_core::calcFiltBaroOffset()
+{
+    // Apply a first order LPF with spike protection
+    baroHgtOffset += 0.1f * constrain_float(baroDataDelayed.hgt + stateStruct.position.z - baroHgtOffset, -5.0f, 5.0f);
+}
+
 // store baro in a history array
 void NavEKF2_core::StoreBaro()
 {

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -629,6 +629,9 @@ private:
     // using a simple observer
     void calcOutputStatesFast();
 
+    // calculate a filtered offset between baro height measurement and EKF height estimate
+    void calcFiltBaroOffset();
+
     // Length of FIFO buffers used for non-IMU sensor data.
     // Must be larger than the time period defined by IMU_BUFFER_LENGTH
     static const uint32_t OBS_BUFFER_LENGTH = 5;