From cfcb76f627d164b3244e9b81b2fe2fe2e54ee045 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 18 Aug 2014 21:23:59 +0200
Subject: [PATCH] Add long-term stable wind estimate

---
 .../estimator_23states.cpp                    | 21 +++++++++++++++++++
 .../estimator_23states.h                      |  3 +++
 2 files changed, 24 insertions(+)

diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
index d1349535d4..d176ccee21 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
@@ -71,6 +71,9 @@ AttPosEKF::AttPosEKF() :
     dtVelPosFilt(0.01f),
     dtHgtFilt(0.01f),
     dtGpsFilt(0.1f),
+    windSpdFiltNorth(0.0f),
+    windSpdFiltEast(0.0f),
+    windSpdFiltAltitude(0.0f),
     fusionModeGPS(0),
     innovVelPos{},
     varInnovVelPos{},
@@ -1657,6 +1660,24 @@ void AttPosEKF::FuseAirspeed()
     // Perform fusion of True Airspeed measurement
     if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f))
     {
+        // Lowpass the output of the wind estimate - we want a long-term
+        // stable estimate, but not start to load into the overall dynamics
+        // of the system (which adjusting covariances would do)
+
+        // Nominally damp to 0.02% of the noise, but reduce the damping for strong altitude variations
+        // assuming equal wind speeds on the same altitude and varying wind speeds on
+        // different altitudes
+        float windFiltCoeff = 0.0002f;
+
+        float altDiff = fabsf(windSpdFiltAltitude - hgtMea);
+
+        // Change filter coefficient based on altitude
+        windFiltCoeff += ConstrainFloat(0.00001f * altDiff, windFiltCoeff, 0.1f);
+
+        windSpdFiltNorth = ((1.0f - windFiltCoeff) * windSpdFiltNorth) + (windFiltCoeff * vwn);
+        windSpdFiltEast = ((1.0f - windFiltCoeff) * windSpdFiltEast) + (windFiltCoeff * vwe);
+        windSpdFiltAltitude = hgtMea;
+
         // Calculate observation jacobians
         SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
         SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f;
diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h
index 12cbc53b8d..6349b03f02 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_23states.h
+++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h
@@ -149,6 +149,9 @@ public:
     float dtVelPosFilt; // average time between position / velocity fusion steps
     float dtHgtFilt; // average time between height measurement updates
     float dtGpsFilt; // average time between gps measurement updates
+    float windSpdFiltNorth; // average wind speed north component
+    float windSpdFiltEast; // average wind speed east component
+    float windSpdFiltAltitude; // the last altitude used to filter wind speed
     uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
     float innovVelPos[6]; // innovation output
     float varInnovVelPos[6]; // innovation variance output