dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #1894 from Zefz/ekf-mc_fly_forward 

AttPosEKF Fix for inhibit mag state for fly-forward for multicopters
This commit is contained in:
Lorenz Meier 2015-03-10 09:11:13 +01:00
parent daab64f9e4 73ac2d90cf
commit 04555f7b8f
5 changed files with 71 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -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);
}
}

38
src/modules/ekf_att_pos_estimator/estimator_22states.cpp
View File

@ -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;
}

15
src/modules/ekf_att_pos_estimator/estimator_22states.h
View File

@ -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();

34
src/modules/ekf_att_pos_estimator/estimator_utilities.cpp
View File

@ -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;
}

24
src/modules/ekf_att_pos_estimator/estimator_utilities.h
View File

@ -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,