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 aad3aa43df..3bb395a870 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 d8e3116b98..5c01286e30 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 e15ded9777..c5517e38b6 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 e2f4c7e822..470eb4e2c2 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 // 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 95b83ead45..c137209ffe 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,