Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Improvements to non-GPS performance 

Change to user adjustable fusion of constant position (as per legacy EKF) instead of constant velocity.
Enable user to specify use of 3-axis magnetometer fusion when operating without aiding.
Don't allow gyro scale factor learning without external aiding data as it can be unreliable
This commit is contained in:
Paul Riseborough 2016-01-20 14:07:10 +11:00 committed by Randy Mackay
parent a9c985bfb3
commit 7e05646316
6 changed files with 35 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -420,12 +420,12 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
AP_GROUPINFO("CHECK_SCALE", 34, NavEKF2, _gpsCheckScaler, CHECK_SCALER_DEFAULT),
// @Param: NOAID_NOISE
// @DisplayName: Non-GPS operation velocity change (m/s)
// @Description: This sets the amount of velocity change that the EKF allows for when operating without external measurements (eg GPs or optical flow). Increasing this parameter makes the EKF attitude estimate less affected by changes in vehicle velocity but also makes the EKF attitude estimate more affected by IMU errors.
// @Range: 0.5 25.0
// @DisplayName: Non-GPS operation position uncertainty (m)
// @Description: This sets the amount of position variation that the EKF allows for when operating without external measurements (eg GPS or optical flow). Increasing this parameter makes the EKF attitude estimate less sensitive to vehicle manoeuvres but more sensitive to IMU errors.
// @Range: 0.5 50.0
// @User: Advanced
// @Units: m/s
AP_GROUPINFO("NOAID_NOISE", 35, NavEKF2, _noaidHorizVelNoise, 10.0f),
AP_GROUPINFO("NOAID_NOISE", 35, NavEKF2, _noaidHorizNoise, 10.0f),
AP_GROUPEND
};

2
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -311,7 +311,7 @@ private:
AP_Int8 _gpsCheck; // Bitmask controlling which preflight GPS checks are bypassed
AP_Int8 _imuMask; // Bitmask of IMUs to instantiate EKF2 for
AP_Int16 _gpsCheckScaler; // Percentage increase to be applied to GPS pre-flight accuracy and drift thresholds
AP_Float _noaidHorizVelNoise; // horizontal velocity measurement noise assuned when synthesised zero velocity measurements are used to constrain attitude drift : m/s
AP_Float _noaidHorizNoise; // horizontal position measurement noise assumed when synthesised zero position measurements are used to constrain attitude drift : m
// Tuning parameters
const float gpsNEVelVarAccScale; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration

3
libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
View File

@ -64,8 +64,7 @@ void NavEKF2_core::setWindMagStateLearningMode()
// Deny mag calibration request if we aren't using the compass, it has been inhibited by the user,
// we do not have an absolute position reference or are on the ground (unless explicitly requested by the user)
// If we do nto have absolute position (eg GPS) then the earth field states cannot be learned
bool magCalDenied = !use_compass() || (frontend->_magCal == 2) || (PV_AidingMode == AID_NONE) || (onGround && frontend->_magCal != 4);
bool magCalDenied = !use_compass() || (frontend->_magCal == 2) ||(onGround && frontend->_magCal != 4);
// Inhibit the magnetic field calibration if not requested or denied
inhibitMagStates = (!magCalRequested || magCalDenied);

12
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -385,12 +385,6 @@ void NavEKF2_core::FuseMagnetometer()
}
}
// inhibit position state modification if we are not aiding
if (PV_AidingMode == AID_NONE) {
Kfusion[6] = 0.0f;
Kfusion[7] = 0.0f;
}
// reset the observation index to 0 (we start by fusing the X measurement)
obsIndex = 0;
@ -810,12 +804,6 @@ void NavEKF2_core::FuseDeclination()
Kfusion[i] = t14*(P[i][17]*t2*t6-P[i][16]*magE*t6*t7);
}
// inhibit position state modification if we are not aiding
if (PV_AidingMode == AID_NONE) {
Kfusion[6] = 0.0f;
Kfusion[7] = 0.0f;
}
// get the magnetic declination
float magDecAng = use_compass() ? _ahrs->get_compass()->get_declination() : 0;

36
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -168,18 +168,21 @@ void NavEKF2_core::SelectVelPosFusion()
// Select height data to be fused from the available baro, range finder and GPS sources
selectHeightForFusion();
// If we are operating without any aiding, fuse in the last known position and zero velocity
// If we are operating without any aiding, fuse in the last known position
// to constrain tilt drift. This assumes a non-manoeuvring vehicle
// Do this to coincide with the height fusion
if (fuseHgtData && PV_AidingMode == AID_NONE) {
gpsDataDelayed.vel.zero();
// only fuse synthetic measurements when rate of change of velocity is less than 1g to reduce attitude errors due to launch acceleration
if (accNavMag < 9.8f) {
fuseVelData = true;
gpsDataDelayed.pos.x = lastKnownPositionNE.x;
gpsDataDelayed.pos.y = lastKnownPositionNE.y;
// when in flight only fuse synthetic measurements when rate of change of velocity is less than 1.1g
// to reduce attitude errors due to launch acceleration
if (accNavMag < 1.1f * GRAVITY_MSS || motorsArmed) {
fusePosData = true;
} else {
fuseVelData = false;
fusePosData = false;
}
fusePosData = false;
fuseVelData = false;
}
// perform fusion
@ -242,17 +245,19 @@ void NavEKF2_core::FuseVelPosNED()
posErr = frontend->gpsPosVarAccScale * accNavMag;
// estimate the GPS Velocity, GPS horiz position and height measurement variances.
// Use different errors if operating without external aiding using an assumed velocity of zero
// Use different errors if operating without external aiding using an assumed position or velocity of zero
if (PV_AidingMode == AID_NONE) {
if (tiltAlignComplete && motorsArmed) {
// This is a compromise between corrections for gyro errors and reducing effect of manoeuvre accelerations on tilt estimate
R_OBS[0] = sq(constrain_float(frontend->_noaidHorizVelNoise, 0.5f, 25.0f));
R_OBS[0] = sq(constrain_float(frontend->_noaidHorizNoise, 0.5f, 50.0f));
} else {
// Use a smaller value to give faster initial alignment
R_OBS[0] = sq(0.5f);
}
R_OBS[1] = R_OBS[0];
R_OBS[2] = R_OBS[0];
R_OBS[3] = R_OBS[0];
R_OBS[4] = R_OBS[0];
for (uint8_t i=0; i<=2; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
} else {
if (gpsSpdAccuracy > 0.0f) {
@ -268,10 +273,10 @@ void NavEKF2_core::FuseVelPosNED()
// For data integrity checks we use the same measurement variances as used to calculate the Kalman gains for all measurements except GPS horizontal velocity
// For horizontal GPs velocity we don't want the acceptance radius to increase with reported GPS accuracy so we use a value based on best GPs perfomrance
// plus a margin for manoeuvres. It is better to reject GPS horizontal velocity errors early
R_OBS[3] = sq(constrain_float(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr);
R_OBS[4] = R_OBS[3];
for (uint8_t i=0; i<=2; i++) R_OBS_DATA_CHECKS[i] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
}
R_OBS[3] = sq(constrain_float(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr);
R_OBS[4] = R_OBS[3];
R_OBS[5] = posDownObsNoise;
for (uint8_t i=3; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
@ -305,7 +310,10 @@ void NavEKF2_core::FuseVelPosNED()
// declare a timeout condition if we have been too long without data or not aiding
posTimeout = (((imuSampleTime_ms - lastPosPassTime_ms) > gpsRetryTime) || PV_AidingMode == AID_NONE);
// use position data if healthy or timed out
if (posHealth || posTimeout) {
if (PV_AidingMode == AID_NONE) {
posHealth = true;
lastPosPassTime_ms = imuSampleTime_ms;
} else if (posHealth || posTimeout) {
posHealth = true;
lastPosPassTime_ms = imuSampleTime_ms;
// if timed out or outside the specified uncertainty radius, reset to the GPS
@ -489,12 +497,6 @@ void NavEKF2_core::FuseVelPosNED()
Kfusion[23] = 0.0f;
}
// inhibit position state modification if we are not aiding
if (PV_AidingMode == AID_NONE) {
Kfusion[6] = 0.0f;
Kfusion[7] = 0.0f;
}
// zero the attitude error state - by definition it is assumed to be zero before each observaton fusion
stateStruct.angErr.zero();

27
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -513,12 +513,7 @@ void NavEKF2_core::UpdateStrapdownEquationsNED()
stateStruct.velocity += delVelNav;
// apply a trapezoidal integration to velocities to calculate position
// if we are not doing aiding, then the horizontal position states are not predicted
if (PV_AidingMode != AID_NONE) {
stateStruct.position += (stateStruct.velocity + lastVelocity) * (imuDataDelayed.delVelDT*0.5f);
} else {
stateStruct.position.z += (stateStruct.velocity.z + lastVelocity.z) * (imuDataDelayed.delVelDT*0.5f);
}
stateStruct.position += (stateStruct.velocity + lastVelocity) * (imuDataDelayed.delVelDT*0.5f);
// accumulate the bias delta angle and time since last reset by an OF measurement arrival
delAngBodyOF += imuDataDelayed.delAng - stateStruct.gyro_bias;
@ -677,7 +672,6 @@ void NavEKF2_core::CovariancePrediction()
for (uint8_t i= 0; i<=8; i++) processNoise[i] = 0.0f;
for (uint8_t i=9; i<=11; i++) processNoise[i] = dAngBiasSigma;
for (uint8_t i=12; i<=14; i++) processNoise[i] = dAngScaleSigma;
processNoise[15] = dVelBiasSigma;
if (expectGndEffectTakeoff) {
processNoise[15] = 0.0f;
} else {
@ -1230,18 +1224,17 @@ void NavEKF2_core::ConstrainVariances()
{
for (uint8_t i=0; i<=2; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0f); // attitude error
for (uint8_t i=3; i<=5; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e3f); // velocities
// If we are not using the horizontal position states, keep their covariances at the initialisation values
if (PV_AidingMode == AID_NONE) {
zeroRows(P,6,7);
zeroCols(P,6,7);
P[6][6] = sq(frontend->_gpsHorizPosNoise);
P[7][7] = P[6][6];
} else {
for (uint8_t i=6; i<=7; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e6f);
}
for (uint8_t i=6; i<=7; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e6f);
P[8][8] = constrain_float(P[8][8],0.0f,1.0e6f); // vertical position
for (uint8_t i=9; i<=11; i++) P[i][i] = constrain_float(P[i][i],0.0f,sq(0.175f * dtEkfAvg)); // delta angle biases
for (uint8_t i=12; i<=14; i++) P[i][i] = constrain_float(P[i][i],0.0f,0.01f); // delta angle scale factors
if (PV_AidingMode != AID_NONE) {
for (uint8_t i=12; i<=14; i++) P[i][i] = constrain_float(P[i][i],0.0f,0.01f); // delta angle scale factors
} else {
// we can't reliably estimate scale factors when there is no aiding data due to transient manoeuvre induced innovations
// so inhibit estimation by keeping covariance elements at zero
zeroRows(P,12,14);
zeroCols(P,12,14);
}
P[15][15] = constrain_float(P[15][15],0.0f,sq(10.0f * dtEkfAvg)); // delta velocity bias
for (uint8_t i=16; i<=18; i++) P[i][i] = constrain_float(P[i][i],0.0f,0.01f); // earth magnetic field
for (uint8_t i=19; i<=21; i++) P[i][i] = constrain_float(P[i][i],0.0f,0.01f); // body magnetic field