Run full update straight after reset, filter wind speed dynamically

3 files changed, 31 insertions, 32 deletions

diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index c384b2566..b7e118d47 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -1220,30 +1220,12 @@ FixedwingEstimator::task_main()
 				} else if (_ekf->statesInitialised) {
 
 					// We're apparently initialized in this case now
-					int check = check_filter_state();
-
-					if (check) {
-						// Let the system re-initialize itself
-						continue;
-					}
+					// check (and reset the filter as needed)
+					(void)check_filter_state();
 
 					// Run the strapdown INS equations every IMU update
 					_ekf->UpdateStrapdownEquationsNED();
-	#if 0
-					// debug code - could be tunred into a filter mnitoring/watchdog function
-					float tempQuat[4];
-
-					for (uint8_t j = 0; j <= 3; j++) tempQuat[j] = states[j];
-
-					quat2eul(eulerEst, tempQuat);
 
-					for (uint8_t j = 0; j <= 2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j];
-
-					if (eulerDif[2] > pi) eulerDif[2] -= 2 * pi;
-
-					if (eulerDif[2] < -pi) eulerDif[2] += 2 * pi;
-
-	#endif
 					// store the predicted states for subsequent use by measurement fusion
 					_ekf->StoreStates(IMUmsec);
 					// Check if on ground - status is used by covariance prediction
diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
index d176ccee2..249cc419e 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
@@ -74,6 +74,7 @@ AttPosEKF::AttPosEKF() :
     windSpdFiltNorth(0.0f),
     windSpdFiltEast(0.0f),
     windSpdFiltAltitude(0.0f),
+    windSpdFiltClimb(0.0f),
     fusionModeGPS(0),
     innovVelPos{},
     varInnovVelPos{},
@@ -1660,22 +1661,30 @@ 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);
+        if (isfinite(windSpdFiltClimb)) {
+            windSpdFiltClimb = ((1.0f - 0.0002f) * windSpdFiltClimb) + (0.0002f * states[6]);
+        } else {
+            windSpdFiltClimb = states[6];
+        }
+
+        if (altDiff < 20.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)
+
+            // Change filter coefficient based on altitude change rate
+            float windFiltCoeff = ConstrainFloat(fabsf(windSpdFiltClimb) / 1000.0f, 0.00005f, 0.2f);
+
+            windSpdFiltNorth = ((1.0f - windFiltCoeff) * windSpdFiltNorth) + (windFiltCoeff * vwn);
+            windSpdFiltEast = ((1.0f - windFiltCoeff) * windSpdFiltEast) + (windFiltCoeff * vwe);
+        } else {
+            windSpdFiltNorth = vwn;
+            windSpdFiltEast = vwe;
+        }
 
-        windSpdFiltNorth = ((1.0f - windFiltCoeff) * windSpdFiltNorth) + (windFiltCoeff * vwn);
-        windSpdFiltEast = ((1.0f - windFiltCoeff) * windSpdFiltEast) + (windFiltCoeff * vwe);
         windSpdFiltAltitude = hgtMea;
 
         // Calculate observation jacobians
@@ -3097,6 +3106,13 @@ void AttPosEKF::ZeroVariables()
     dVelIMU.zero();
     lastGyroOffset.zero();
 
+    windSpdFiltNorth = 0.0f;
+    windSpdFiltEast = 0.0f;
+    // setting the altitude to zero will give us a higher
+    // gain to adjust faster in the first step
+    windSpdFiltAltitude = 0.0f;
+    windSpdFiltClimb = 0.0f;
+
     for (unsigned i = 0; i < data_buffer_size; i++) {
 
         for (unsigned j = 0; j < n_states; j++) {
diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h
index 6349b03f0..f8bb7a9c4 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_23states.h
+++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h
@@ -152,6 +152,7 @@ public:
     float windSpdFiltNorth; // average wind speed north component
     float windSpdFiltEast; // average wind speed east component
     float windSpdFiltAltitude; // the last altitude used to filter wind speed
+    float windSpdFiltClimb; // filtered climb rate
     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