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

AP_NavEKF2: Ensure consistent position and velocity fusion rates when not using GPS 

This sets the fusion of the synthetic position and velocity to occur at the same time as the barometer

This makes filter tuning more consistent between GPS and non-GPS useage
This commit is contained in:
Paul Riseborough 2015-10-21 16:20:52 +11:00 committed by Andrew Tridgell
parent 1c347e8859
commit b08817554e
2 changed files with 23 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -599,23 +599,6 @@ void NavEKF2_core::readGpsData()
}
}
}
// If not aiding we synthesise the GPS measurements at the last known position
if (PV_AidingMode == AID_NONE) {
if (imuSampleTime_ms - gpsDataNew.time_ms > 200) {
gpsDataNew.pos.x = lastKnownPositionNE.x;
gpsDataNew.pos.y = lastKnownPositionNE.y;
gpsDataNew.time_ms = imuSampleTime_ms-frontend._gpsDelay_ms;
// Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame
// This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors
gpsDataNew.time_ms = roundToNearest(gpsDataNew.time_ms, frontend.fusionTimeStep_ms);
// Prevent time delay exceeding age of oldest IMU data in the buffer
gpsDataNew.time_ms = max(gpsDataNew.time_ms,imuDataDelayed.time_ms);
// save measurement to buffer to be fused later
StoreGPS();
}
}
}

38
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -119,11 +119,9 @@ void NavEKF2_core::SelectVelPosFusion()
readGpsData();
// Determine if we need to fuse position and velocity data on this time step
if (RecallGPS() && PV_AidingMode != AID_RELATIVE) {
if (RecallGPS() && PV_AidingMode == AID_ABSOLUTE) {
// Don't fuse velocity data if GPS doesn't support it
// If no aiding is avaialble, then we use zeroed GPS position and elocity data to constrain
// tilt errors assuming that the vehicle is not accelerating
if (frontend._fusionModeGPS <= 1 || PV_AidingMode == AID_NONE) {
if (frontend._fusionModeGPS <= 1) {
fuseVelData = true;
} else {
fuseVelData = false;
@ -151,11 +149,26 @@ void NavEKF2_core::SelectVelPosFusion()
fuseHgtData = true;
}
// If we are operating without any aiding, fuse in the last known position and zero velocity
// 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();
gpsDataDelayed.pos.x = lastKnownPositionNE.x;
gpsDataDelayed.pos.y = lastKnownPositionNE.y;
fuseVelData = true;
fusePosData = true;
}
// perform fusion
if (fuseVelData || fusePosData || fuseHgtData) {
// ensure that the covariance prediction is up to date before fusing data
if (!covPredStep) CovariancePrediction();
FuseVelPosNED();
// clear the flags to prevent repeated fusion of the same data
fuseVelData = false;
fuseHgtData = false;
fusePosData = false;
}
}
@ -198,17 +211,12 @@ void NavEKF2_core::FuseVelPosNED()
if (useAirspeed()) gpsRetryTime = frontend.gpsRetryTimeUseTAS_ms;
else gpsRetryTime = frontend.gpsRetryTimeNoTAS_ms;
// form the observation vector and zero velocity and horizontal position observations if in constant position mode
// If in constant velocity mode, hold the last known horizontal velocity vector
if (PV_AidingMode == AID_ABSOLUTE) {
observation[0] = gpsDataDelayed.vel.x + gpsVelGlitchOffset.x;
observation[1] = gpsDataDelayed.vel.y + gpsVelGlitchOffset.y;
observation[2] = gpsDataDelayed.vel.z;
observation[3] = gpsDataDelayed.pos.x + gpsPosGlitchOffsetNE.x;
observation[4] = gpsDataDelayed.pos.y + gpsPosGlitchOffsetNE.y;
} else if (PV_AidingMode == AID_NONE) {
for (uint8_t i=0; i<=4; i++) observation[i] = 0.0f;
}
// form the observation vector
observation[0] = gpsDataDelayed.vel.x + gpsVelGlitchOffset.x;
observation[1] = gpsDataDelayed.vel.y + gpsVelGlitchOffset.y;
observation[2] = gpsDataDelayed.vel.z;
observation[3] = gpsDataDelayed.pos.x + gpsPosGlitchOffsetNE.x;
observation[4] = gpsDataDelayed.pos.y + gpsPosGlitchOffsetNE.y;
observation[5] = -baroDataDelayed.hgt;
// calculate additional error in GPS position caused by manoeuvring