From 807e02b4a0dad03bd26d1d39ba5af3c5bae94750 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Sun, 8 Mar 2015 12:13:56 +0100 Subject: [PATCH 1/6] AttPosEKF: Direct to EKF whether the vehicle is flying like a FixedWing or not --- .../ekf_att_pos_estimator_main.cpp | 3 +++ .../estimator_22states.cpp | 21 ++++++++++++------- .../estimator_22states.h | 14 +++++++++++-- 3 files changed, 29 insertions(+), 9 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 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..a22253dd26 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -185,7 +185,8 @@ AttPosEKF::AttPosEKF() : minFlowRng(0.0f), moCompR_LOS(0.0f), - _onGround(true) + _isFixedWing(false), + _onGround(true) { memset(&last_ekf_error, 0, sizeof(last_ekf_error)); @@ -196,6 +197,7 @@ AttPosEKF::AttPosEKF() : AttPosEKF::~AttPosEKF() { + //dtor } void AttPosEKF::InitialiseParameters() @@ -2003,12 +2005,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; @@ -3317,3 +3319,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..8dd1d74e91 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,8 @@ 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) }; uint32_t millis(); From 11568c77d49285b7ea376e2696cdce1fdb57d13c Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Sun, 8 Mar 2015 12:14:47 +0100 Subject: [PATCH 2/6] VectorMath: Add scalar division to custom EKF vector math --- .../ekf_att_pos_estimator/estimator_utilities.cpp | 10 ++++++++++ .../ekf_att_pos_estimator/estimator_utilities.h | 1 + 2 files changed, 11 insertions(+) diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp index e2f4c7e822..64a25aaba2 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp @@ -198,3 +198,13 @@ void swap_var(float &d1, float &d2) d1 = d2; d2 = tmp; } + +// overload / operator to provide a vector scalar division +Vector3f operator/(const Vector3f &vec, const float scalar) +{ + 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..b2d790e271 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -89,6 +89,7 @@ Vector3f operator*( Mat3f matIn, Vector3f vecIn); Mat3f operator*( Mat3f matIn1, Mat3f matIn2); Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2); Vector3f operator*(Vector3f vecIn1, float sclIn1); +Vector3f operator/(const Vector3f &vec, const float scalar); void swap_var(float &d1, float &d2); From 34f6cf9eb6e5374b9fbeed914509f44d2a0a914e Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Sun, 8 Mar 2015 12:15:39 +0100 Subject: [PATCH 3/6] AttPosEKF: Inhibit mag state if not fixed wing and not accelerating forwards --- .../estimator_22states.cpp | 19 ++++++++++++------- .../estimator_22states.h | 1 + 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 a22253dd26..0e931e6666 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 8dd1d74e91..c5517e38b6 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(); From 1dc7d4905c1c19e6c6a35440866de375c3a929af Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Sun, 8 Mar 2015 12:26:59 +0100 Subject: [PATCH 4/6] VectorMath: Optimization by passing vector by reference instead of value --- .../estimator_utilities.cpp | 30 ++++++++----------- .../estimator_utilities.h | 23 +++++++------- 2 files changed, 22 insertions(+), 31 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp index 64a25aaba2..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,13 +193,6 @@ Vector3f operator*(float sclIn1, Vector3f vecIn1) return vecOut; } -void swap_var(float &d1, float &d2) -{ - float tmp = d1; - d1 = d2; - d2 = tmp; -} - // overload / operator to provide a vector scalar division Vector3f operator/(const Vector3f &vec, const float scalar) { diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h index b2d790e271..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,20 +78,18 @@ 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); +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); -void swap_var(float &d1, float &d2); - enum GPS_FIX { GPS_FIX_NOFIX = 0, GPS_FIX_2D = 2, From cdbd6872ed24be85f35789fdc0c6da27152411da Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Sun, 8 Mar 2015 22:00:38 +0100 Subject: [PATCH 5/6] AttPosEKF: Fix inverted logic for inhibitMagStates --- src/modules/ekf_att_pos_estimator/estimator_22states.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 0e931e6666..cd0ef206eb 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -2552,7 +2552,7 @@ void AttPosEKF::setOnGround(const bool isLanded) bool isMovingForward = _accNavMagHorizontal > 0.5f; // don't update magnetic field states if on ground or not using compass - inhibitMagStates = useCompass && !_onGround && (_isFixedWing || isMovingForward); + 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) { From 73ac2d90cf991260e7bb74605ff5d7e9aca3c58a Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Sun, 8 Mar 2015 22:18:28 +0100 Subject: [PATCH 6/6] AttPosEKF: Compile fix for missing braces --- src/modules/ekf_att_pos_estimator/estimator_22states.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index cd0ef206eb..5c01286e30 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -2552,7 +2552,7 @@ void AttPosEKF::setOnGround(const bool isLanded) bool isMovingForward = _accNavMagHorizontal > 0.5f; // don't update magnetic field states if on ground or not using compass - inhibitMagStates = !useCompass || _onGround) || (!_isFixedWing && !isMovingForward); + 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) {