Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 01:33:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Reduced heading drift on ground

Browse Source
This commit is contained in:
Paul Riseborough 2013-12-31 07:56:28 +11:00 committed by Andrew Tridgell
parent 63d8b1bb0b
commit 1647ba9bd0
1 changed files with 42 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -19,7 +19,7 @@ NavEKF::NavEKF(const AP_AHRS &ahrs, AP_Baro &baro) :
_baro(baro),
useAirspeed(true),
useCompass(true),
fusionModeGPS(1), // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
fusionModeGPS(0), // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
covTimeStepMax(0.07f), // maximum time (sec) between covariance prediction updates
covDelAngMax(0.05f), // maximum delta angle between covariance prediction updates
TASmsecMax(333), // maximum allowed interal between airspeed measurement updates
@ -260,7 +260,7 @@ void NavEKF::UpdateStrapdownEquationsNED()
prevDelAng = correctedDelAng;
// Apply corrections for earths rotation rate and coning errors
// * and + operators have been overloaded
// % * - and + operators have been overloaded
correctedDelAng = correctedDelAng - prevTnb * earthRateNED*dtIMU + (prevDelAng % correctedDelAng) * 8.333333333333333e-2f;
// Convert the rotation vector to its equivalent quaternion
@ -396,7 +396,14 @@ void NavEKF::CovariancePrediction()
dvz_b = states[15];
daxCov = sq(dt*1.4544411e-2f);
dayCov = sq(dt*1.4544411e-2f);
dazCov = sq(dt*1.4544411e-2f);
if (!onGround)
{
dazCov = sq(dt*1.4544411e-2f);
}
else
{
dazCov = sq(dt*1.4544411e-1f); //reduce heading drift when on ground
}
dvxCov = sq(dt*0.5f);
dvyCov = sq(dt*0.5f);
dvzCov = sq(dt*0.5f);
@ -1041,15 +1048,10 @@ void NavEKF::CovariancePrediction()
nextP[i][i] = nextP[i][i] + processNoise[i];
}
// If on ground, inhibit accelerometer bias, wind and magnetic field state updates by
// If on ground or no compasss fitted, inhibit magnetic field state updates by
// setting the corresponding covariance terms to zero
// This is a quick hack - for efficiency should really not calculate terms above when this condition is true
if (onGround)
if (onGround || !useCompass)
{
zeroRows(nextP,13,15);
zeroCols(nextP,13,15);
zeroRows(nextP,16,17);
zeroCols(nextP,16,17);
zeroRows(nextP,18,23);
zeroCols(nextP,18,23);
}
@ -1062,18 +1064,9 @@ void NavEKF::CovariancePrediction()
zeroCols(nextP,13,15);
}
// If on ground or no magnnextP[i][j] + nextP[j][i]etometer fitted, inhibit magnetometer bias updates by
// setting the coresponding covariance terms to zero. To be efficient, the
// corresponding terms should also not be calculated above
if (onGround || !useCompass)
{
zeroRows(nextP,18,23);
zeroCols(nextP,18,23);
}
// If on ground or not using airspeed sensing, inhibit wind velocity
// covariance growth. To be efficient, the corresponding terms should also
// not be calculated above
// covariance growth.
if (onGround || !useAirspeed)
{
zeroRows(nextP,16,17);
@ -1135,6 +1128,7 @@ void NavEKF::FuseVelPosNED()
bool fuseData[6] = {false,false,false,false,false,false};
uint8_t stateIndex;
uint8_t obsIndex;
uint8_t indexLimit; // used to prevent access to wind and magnetic field states and variances when on ground
// declare variables used by state and covariance update calculations
float velErr;
@ -1257,6 +1251,15 @@ void NavEKF::FuseVelPosNED()
{
fuseData[5] = true;
}
// Limit access to first 13 states when on ground.
if (!onGround)
{
indexLimit = 23;
}
else
{
indexLimit = 12;
}
// Fuse measurements sequentially
for (obsIndex=0; obsIndex<=5; obsIndex++)
{
@ -1283,12 +1286,12 @@ void NavEKF::FuseVelPosNED()
SK = 1.0f/varInnovVelPos[obsIndex];
// Check the innovation for consistency and don't fuse if > TBD Sigma
// Currently disabled for testing
for (uint8_t i= 0; i<=23; i++)
for (uint8_t i= 0; i<=indexLimit; i++)
{
Kfusion[i] = P[i][stateIndex]*SK;
}
// Calculate state corrections and re-normalise the quaternions
for (uint8_t i = 0; i<=23; i++)
for (uint8_t i = 0; i<=indexLimit; i++)
{
states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex];
}
@ -1301,16 +1304,16 @@ void NavEKF::FuseVelPosNED()
// Update the covariance - take advantage of direct observation of a
// single state at index = stateIndex to reduce computations
// Optimised implementation of standard equation P = (I - K*H)*P;
for (uint8_t i= 0; i<=23; i++)
for (uint8_t i= 0; i<=indexLimit; i++)
{
for (uint8_t j= 0; j<=23; j++)
for (uint8_t j= 0; j<=indexLimit; j++)
{
KHP[i][j] = Kfusion[i] * P[stateIndex][j];
}
}
for (uint8_t i= 0; i<=23; i++)
for (uint8_t i= 0; i<=indexLimit; i++)
{
for (uint8_t j= 0; j<=23; j++)
for (uint8_t j= 0; j<=indexLimit; j++)
{
P[i][j] = P[i][j] - KHP[i][j];
}
@ -1344,6 +1347,7 @@ void NavEKF::FuseMagnetometer()
Vector6 SK_MY;
Vector6 SK_MZ;
Vector24 Kfusion;
uint8_t indexLimit; // used to prevent access to wind and magnetic field states and variances when on ground
// Perform sequential fusion of Magnetometer measurements.
// This assumes that the errors in the different components are
@ -1555,8 +1559,17 @@ void NavEKF::FuseMagnetometer()
// Check the innovation for consistency and don't fuse if > 5Sigma
if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0f)
{
// Limit access to first 13 states when on ground.
if (!onGround)
{
indexLimit = 23;
}
else
{
indexLimit = 12;
}
// correct the state vector
for (uint8_t j= 0; j<=23; j++)
for (uint8_t j= 0; j<=indexLimit; j++)
{
states[j] = states[j] - Kfusion[j] * innovMag[obsIndex];
}
@ -1600,9 +1613,9 @@ void NavEKF::FuseMagnetometer()
}
}
}
for (uint8_t i = 0; i<=23; i++)
for (uint8_t i = 0; i<=indexLimit; i++)
{
for (uint8_t j = 0; j<=23; j++)
for (uint8_t j = 0; j<=indexLimit; j++)
{
P[i][j] = P[i][j] - KHP[i][j];
}