AttPosEKF: Inhibit mag state if not fixed wing and not accelerating forwards

2 files changed, 13 insertions, 7 deletions

diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
index a22253dd2..0e931e666 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
@@ -186,7 +186,8 @@ AttPosEKF::AttPosEKF() :
     moCompR_LOS(0.0f),
 
     _isFixedWing(false),
-    _onGround(true)
+    _onGround(true),
+    _accNavMagHorizontal(0.0f)
 {
 
     memset(&last_ekf_error, 0, sizeof(last_ekf_error));
@@ -350,6 +351,11 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
     // variance estimation)
     accNavMag = delVelNav.length()/dtIMU;
 
+    //First order low-pass filtered magnitude of horizontal nav acceleration
+    Vector3f derivativeNav = (delVelNav / dtIMU);
+    float derivativeVelNavMagnitude = sqrtf(sq(derivativeNav.x) + sq(derivativeNav.y));
+    _accNavMagHorizontal = _accNavMagHorizontal * 0.95f + derivativeVelNavMagnitude * 0.05f;
+
     // If calculating position save previous velocity
     float lastVelocity[3];
     lastVelocity[0] = states[4];
@@ -2539,15 +2545,14 @@ void AttPosEKF::setOnGround(const bool isLanded)
     if (_onGround || !useAirspeed) {
         inhibitWindStates = true;
     } else {
-        inhibitWindStates =false;
+        inhibitWindStates = false;
     }
 
+    //Check if we are accelerating forward, only then is the mag offset is observable
+    bool isMovingForward = _accNavMagHorizontal > 0.5f;
+
     // don't update magnetic field states if on ground or not using compass
-    if (_onGround || !useCompass) {
-        inhibitMagStates = true;
-    } else {
-        inhibitMagStates = false;
-    }
+    inhibitMagStates = useCompass && !_onGround && (_isFixedWing || isMovingForward);
 
     // don't update terrain offset state if there is no range finder and flying at low velocity or without GPS
     if ((_onGround || !useGPS) && !useRangeFinder) {
diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h
index 8dd1d74e9..c5517e38b 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_22states.h
+++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h
@@ -413,6 +413,7 @@ protected:
 private:
     bool _isFixedWing;               ///< True if the vehicle is a fixed-wing frame type
     bool _onGround;                  ///< boolean true when the flight vehicle is on the ground (not flying)
+    float _accNavMagHorizontal;      ///< First-order low-pass filtered rate of change maneuver velocity
 };
 
 uint32_t millis();