Merge pull request #1894 from Zefz/ekf-mc_fly_forward

AttPosEKF Fix for inhibit mag state for fly-forward for multicopters

5 files changed, 71 insertions, 43 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 aad3aa43d..3bb395a87 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
@@ -354,6 +354,9 @@ void AttitudePositionEstimatorEKF::vehicle_status_poll()
 	if (vstatus_updated) {
 
 		orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus);
+
+		//Tell EKF that the vehicle is a fixed wing or multi-rotor
+		_ekf->setIsFixedWing(!_vstatus.is_rotary_wing);
 	}
 }
 
diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
index d8e3116b9..5c01286e3 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
@@ -185,7 +185,9 @@ AttPosEKF::AttPosEKF() :
     minFlowRng(0.0f),
     moCompR_LOS(0.0f),
 
-    _onGround(true)    
+    _isFixedWing(false),
+    _onGround(true),
+    _accNavMagHorizontal(0.0f)
 {
 
     memset(&last_ekf_error, 0, sizeof(last_ekf_error));
@@ -196,6 +198,7 @@ AttPosEKF::AttPosEKF() :
 
 AttPosEKF::~AttPosEKF()
 {
+    //dtor
 }
 
 void AttPosEKF::InitialiseParameters()
@@ -348,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];
@@ -2003,12 +2011,12 @@ void AttPosEKF::FuseOptFlow()
                 K_LOS[1][15] = 0.0f;
             }
             if (inhibitMagStates) {
-            K_LOS[1][16] = SK_LOS[0]*(P[16][0]*tempVar[1] + P[16][1]*tempVar[2] - P[16][2]*tempVar[3] + P[16][3]*tempVar[4] + P[16][5]*tempVar[5] - P[16][6]*tempVar[6] - P[16][9]*tempVar[7] + P[16][4]*tempVar[8]);
-            K_LOS[1][17] = SK_LOS[0]*(P[17][0]*tempVar[1] + P[17][1]*tempVar[2] - P[17][2]*tempVar[3] + P[17][3]*tempVar[4] + P[17][5]*tempVar[5] - P[17][6]*tempVar[6] - P[17][9]*tempVar[7] + P[17][4]*tempVar[8]);
-            K_LOS[1][18] = SK_LOS[0]*(P[18][0]*tempVar[1] + P[18][1]*tempVar[2] - P[18][2]*tempVar[3] + P[18][3]*tempVar[4] + P[18][5]*tempVar[5] - P[18][6]*tempVar[6] - P[18][9]*tempVar[7] + P[18][4]*tempVar[8]);
-            K_LOS[1][19] = SK_LOS[0]*(P[19][0]*tempVar[1] + P[19][1]*tempVar[2] - P[19][2]*tempVar[3] + P[19][3]*tempVar[4] + P[19][5]*tempVar[5] - P[19][6]*tempVar[6] - P[19][9]*tempVar[7] + P[19][4]*tempVar[8]);
-            K_LOS[1][20] = SK_LOS[0]*(P[20][0]*tempVar[1] + P[20][1]*tempVar[2] - P[20][2]*tempVar[3] + P[20][3]*tempVar[4] + P[20][5]*tempVar[5] - P[20][6]*tempVar[6] - P[20][9]*tempVar[7] + P[20][4]*tempVar[8]);
-            K_LOS[1][21] = SK_LOS[0]*(P[21][0]*tempVar[1] + P[21][1]*tempVar[2] - P[21][2]*tempVar[3] + P[21][3]*tempVar[4] + P[21][5]*tempVar[5] - P[21][6]*tempVar[6] - P[21][9]*tempVar[7] + P[21][4]*tempVar[8]);
+                K_LOS[1][16] = SK_LOS[0]*(P[16][0]*tempVar[1] + P[16][1]*tempVar[2] - P[16][2]*tempVar[3] + P[16][3]*tempVar[4] + P[16][5]*tempVar[5] - P[16][6]*tempVar[6] - P[16][9]*tempVar[7] + P[16][4]*tempVar[8]);
+                K_LOS[1][17] = SK_LOS[0]*(P[17][0]*tempVar[1] + P[17][1]*tempVar[2] - P[17][2]*tempVar[3] + P[17][3]*tempVar[4] + P[17][5]*tempVar[5] - P[17][6]*tempVar[6] - P[17][9]*tempVar[7] + P[17][4]*tempVar[8]);
+                K_LOS[1][18] = SK_LOS[0]*(P[18][0]*tempVar[1] + P[18][1]*tempVar[2] - P[18][2]*tempVar[3] + P[18][3]*tempVar[4] + P[18][5]*tempVar[5] - P[18][6]*tempVar[6] - P[18][9]*tempVar[7] + P[18][4]*tempVar[8]);
+                K_LOS[1][19] = SK_LOS[0]*(P[19][0]*tempVar[1] + P[19][1]*tempVar[2] - P[19][2]*tempVar[3] + P[19][3]*tempVar[4] + P[19][5]*tempVar[5] - P[19][6]*tempVar[6] - P[19][9]*tempVar[7] + P[19][4]*tempVar[8]);
+                K_LOS[1][20] = SK_LOS[0]*(P[20][0]*tempVar[1] + P[20][1]*tempVar[2] - P[20][2]*tempVar[3] + P[20][3]*tempVar[4] + P[20][5]*tempVar[5] - P[20][6]*tempVar[6] - P[20][9]*tempVar[7] + P[20][4]*tempVar[8]);
+                K_LOS[1][21] = SK_LOS[0]*(P[21][0]*tempVar[1] + P[21][1]*tempVar[2] - P[21][2]*tempVar[3] + P[21][3]*tempVar[4] + P[21][5]*tempVar[5] - P[21][6]*tempVar[6] - P[21][9]*tempVar[7] + P[21][4]*tempVar[8]);
             } else {
                 for (uint8_t i = 16; i < EKF_STATE_ESTIMATES; i++) {
                     K_LOS[1][i] = 0.0f;
@@ -2537,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) {
@@ -3317,3 +3324,8 @@ void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error)
     memcpy(last_error, &last_ekf_error, sizeof(*last_error));
     memset(&last_ekf_error, 0, sizeof(last_ekf_error));
 }
+
+void AttPosEKF::setIsFixedWing(const bool fixedWing)
+{
+    _isFixedWing = fixedWing;
+}
diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h
index e15ded977..c5517e38b 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_22states.h
+++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h
@@ -372,6 +372,16 @@ public:
 
     void InitializeDynamic(float (&initvelNED)[3], float declination);
 
+    /**
+    * @brief
+    *   Tells the EKF wheter the vehicle is a fixed wing frame or not.
+    *   This changes the way the EKF fuses position or attitude calulations
+    *   by making some assumptions on movement.
+    * @param fixedWing
+    *   true if the vehicle moves like a Fixed Wing, false otherwise
+    **/
+    void setIsFixedWing(const bool fixedWing);
+
 protected:
 
     /**
@@ -401,8 +411,9 @@ protected:
     void ResetStoredStates();
     
 private:
-    bool _onGround;    ///< boolean true when the flight vehicle is on the ground (not flying)
-
+    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();
diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp
index e2f4c7e82..470eb4e2c 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp
@@ -38,6 +38,7 @@
  */
 
 #include "estimator_utilities.h"
+#include <algorithm>
 
 // Define EKF_DEBUG here to enable the debug print calls
 // if the macro is not set, these will be completely
@@ -104,17 +105,17 @@ void Mat3f::identity() {
     z.z = 1.0f;
 }
 
-Mat3f Mat3f::transpose(void) const
+Mat3f Mat3f::transpose() const
 {
     Mat3f ret = *this;
-    swap_var(ret.x.y, ret.y.x);
-    swap_var(ret.x.z, ret.z.x);
-    swap_var(ret.y.z, ret.z.y);
+    std::swap(ret.x.y, ret.y.x);
+    std::swap(ret.x.z, ret.z.x);
+    std::swap(ret.y.z, ret.z.y);
     return ret;
 }
 
 // overload + operator to provide a vector addition
-Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2)
+Vector3f operator+(const Vector3f &vecIn1, const Vector3f &vecIn2)
 {
     Vector3f vecOut;
     vecOut.x = vecIn1.x + vecIn2.x;
@@ -124,7 +125,7 @@ Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2)
 }
 
 // overload - operator to provide a vector subtraction
-Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2)
+Vector3f operator-(const Vector3f &vecIn1, const Vector3f &vecIn2)
 {
     Vector3f vecOut;
     vecOut.x = vecIn1.x - vecIn2.x;
@@ -134,7 +135,7 @@ Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2)
 }
 
 // overload * operator to provide a matrix vector product
-Vector3f operator*( Mat3f matIn, Vector3f vecIn)
+Vector3f operator*(const Mat3f &matIn, const Vector3f &vecIn)
 {
     Vector3f vecOut;
     vecOut.x = matIn.x.x*vecIn.x + matIn.x.y*vecIn.y + matIn.x.z*vecIn.z;
@@ -144,7 +145,7 @@ Vector3f operator*( Mat3f matIn, Vector3f vecIn)
 }
 
 // overload * operator to provide a matrix product
-Mat3f operator*( Mat3f matIn1, Mat3f matIn2)
+Mat3f operator*(const Mat3f &matIn1, const Mat3f &matIn2)
 {
     Mat3f matOut;
     matOut.x.x = matIn1.x.x*matIn2.x.x + matIn1.x.y*matIn2.y.x + matIn1.x.z*matIn2.z.x;
@@ -163,7 +164,7 @@ Mat3f operator*( Mat3f matIn1, Mat3f matIn2)
 }
 
 // overload % operator to provide a vector cross product
-Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2)
+Vector3f operator%(const Vector3f &vecIn1, const Vector3f &vecIn2)
 {
     Vector3f vecOut;
     vecOut.x = vecIn1.y*vecIn2.z - vecIn1.z*vecIn2.y;
@@ -173,7 +174,7 @@ Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2)
 }
 
 // overload * operator to provide a vector scaler product
-Vector3f operator*(Vector3f vecIn1, float sclIn1)
+Vector3f operator*(const Vector3f &vecIn1, const float sclIn1)
 {
     Vector3f vecOut;
     vecOut.x = vecIn1.x * sclIn1;
@@ -183,7 +184,7 @@ Vector3f operator*(Vector3f vecIn1, float sclIn1)
 }
 
 // overload * operator to provide a vector scaler product
-Vector3f operator*(float sclIn1, Vector3f vecIn1)
+Vector3f operator*(float sclIn1, const Vector3f &vecIn1)
 {
     Vector3f vecOut;
     vecOut.x = vecIn1.x * sclIn1;
@@ -192,9 +193,12 @@ Vector3f operator*(float sclIn1, Vector3f vecIn1)
     return vecOut;
 }
 
-void swap_var(float &d1, float &d2)
+// overload / operator to provide a vector scalar division
+Vector3f operator/(const Vector3f &vec, const float scalar)
 {
-    float tmp = d1;
-    d1 = d2;
-    d2 = tmp;
+    Vector3f vecOut;
+    vecOut.x = vec.x / scalar;
+    vecOut.y = vec.y / scalar;
+    vecOut.z = vec.z / scalar;
+    return vecOut;
 }
diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h
index 95b83ead4..c137209ff 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h
+++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h
@@ -52,7 +52,6 @@
 
 class Vector3f
 {
-private:
 public:
     float x;
     float y;
@@ -64,8 +63,8 @@ public:
     z(c)
     {}
 
-    float length(void) const;
-    void zero(void);
+    float length() const;
+    void zero();
 };
 
 class Mat3f
@@ -79,18 +78,17 @@ public:
     Mat3f();
 
     void identity();
-    Mat3f transpose(void) const;
+    Mat3f transpose() const;
 };
 
-Vector3f operator*(float sclIn1, Vector3f vecIn1);
-Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2);
-Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2);
-Vector3f operator*( Mat3f matIn, Vector3f vecIn);
-Mat3f operator*( Mat3f matIn1, Mat3f matIn2);
-Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2);
-Vector3f operator*(Vector3f vecIn1, float sclIn1);
-
-void swap_var(float &d1, float &d2);
+Vector3f operator*(const float sclIn1, const Vector3f &vecIn1);
+Vector3f operator+(const Vector3f &vecIn1, const Vector3f &vecIn2);
+Vector3f operator-(const Vector3f &vecIn1, const Vector3f &vecIn2);
+Vector3f operator*(const Mat3f &matIn, const Vector3f &vecIn);
+Mat3f operator*(const Mat3f &matIn1, const Mat3f &matIn2);
+Vector3f operator%(const Vector3f &vecIn1, const Vector3f &vecIn2);
+Vector3f operator*(const Vector3f &vecIn1, const float sclIn1);
+Vector3f operator/(const Vector3f &vec, const float scalar);
 
 enum GPS_FIX {
     GPS_FIX_NOFIX = 0,