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

AP_NavEKF2: Reduce angle errors when flying without GPS 

Turn off aiding during >1g manoeuvres when not using GPS
Use larger velocity and position uncertainties when flying without GPS
This commit is contained in:
Paul Riseborough 2015-10-23 13:41:30 +11:00 committed by Andrew Tridgell
parent 1eaf318b9b
commit cd5ec3a3e0
1 changed files with 32 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -156,8 +156,14 @@ void NavEKF2_core::SelectVelPosFusion()
gpsDataDelayed.vel.zero();
gpsDataDelayed.pos.x = lastKnownPositionNE.x;
gpsDataDelayed.pos.y = lastKnownPositionNE.y;
fuseVelData = true;
fusePosData = true;
// 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) {
fusePosData = true;
fuseVelData = true;
} else {
fusePosData = false;
fuseVelData = false;
}
}
// perform fusion
@ -225,18 +231,30 @@ void NavEKF2_core::FuseVelPosNED()
// estimate the GPS Velocity, GPS horiz position and height measurement variances.
// if the GPS is able to report a speed error, we use it to adjust the observation noise for GPS velocity
// otherwise we scale it using manoeuvre acceleration
if (gpsSpdAccuracy > 0.0f) {
// use GPS receivers reported speed accuracy - floor at value set by gps noise parameter
R_OBS[0] = sq(constrain_float(gpsSpdAccuracy, frontend._gpsHorizVelNoise, 50.0f));
R_OBS[2] = sq(constrain_float(gpsSpdAccuracy, frontend._gpsVertVelNoise, 50.0f));
// Use different errors if flying without GPS using synthetic position and velocity data
if (PV_AidingMode == AID_NONE && inFlight) {
// Assume the vehicle will be flown with velocity changes less than 10 m/s in this mode (realistic for indoor use)
// This is a compromise between corrections for gyro errors and reducing angular errors due to maneouvres
R_OBS[0] = sq(10.0f);
R_OBS[1] = R_OBS[0];
R_OBS[2] = R_OBS[0];
// Assume a large position uncertainty so as to contrain position states in this mode but minimise angular errors due to manoeuvres
R_OBS[3] = sq(25.0f);
R_OBS[4] = R_OBS[3];
} else {
// calculate additional error in GPS velocity caused by manoeuvring
R_OBS[0] = sq(constrain_float(frontend._gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend.gpsNEVelVarAccScale * accNavMag);
R_OBS[2] = sq(constrain_float(frontend._gpsVertVelNoise, 0.05f, 5.0f)) + sq(frontend.gpsDVelVarAccScale * accNavMag);
if (gpsSpdAccuracy > 0.0f) {
// use GPS receivers reported speed accuracy if available and floor at value set by gps noise parameter
R_OBS[0] = sq(constrain_float(gpsSpdAccuracy, frontend._gpsHorizVelNoise, 50.0f));
R_OBS[2] = sq(constrain_float(gpsSpdAccuracy, frontend._gpsVertVelNoise, 50.0f));
} else {
// calculate additional error in GPS velocity caused by manoeuvring
R_OBS[0] = sq(constrain_float(frontend._gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend.gpsNEVelVarAccScale * accNavMag);
R_OBS[2] = sq(constrain_float(frontend._gpsVertVelNoise, 0.05f, 5.0f)) + sq(frontend.gpsDVelVarAccScale * accNavMag);
}
R_OBS[1] = R_OBS[0];
R_OBS[3] = sq(constrain_float(frontend._gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr);
R_OBS[4] = R_OBS[3];
}
R_OBS[1] = R_OBS[0];
R_OBS[3] = sq(constrain_float(frontend._gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr);
R_OBS[4] = R_OBS[3];
R_OBS[5] = sq(constrain_float(frontend._baroAltNoise, 0.1f, 10.0f));
// reduce weighting (increase observation noise) on baro if we are likely to be in ground effect
@ -312,7 +330,8 @@ void NavEKF2_core::FuseVelPosNED()
if (fuseVelData) {
// test velocity measurements
uint8_t imax = 2;
if (frontend._fusionModeGPS == 1) {
// Don't fuse vertical velocity observations if inhibited by the user or if we are using synthetic data
if (frontend._fusionModeGPS >= 1 || PV_AidingMode != AID_ABSOLUTE) {
imax = 1;
}
float innovVelSumSq = 0; // sum of squares of velocity innovations