From 4c72a14e2208747b56c3cfb2a69bc74f677dc1c7 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p-riseborough@live.com.au>
Date: Thu, 12 Nov 2015 18:29:53 +1100
Subject: [PATCH] AP_NavEKF2: Add function to LPF and spike filter baro data

---
 libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 9 +++++++++
 libraries/AP_NavEKF2/AP_NavEKF2_core.h           | 3 +++
 2 files changed, 12 insertions(+)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
index 2c8aacbaf7..8fdd1b51e5 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
+++ b/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()
 {
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h
index 5707ea8237..bd90a061d3 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h
+++ b/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;