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

AP_NavEKF2: Improve non-GPS in-flight attitude accuracy 

The non-GPS mode was not being activated for small height gains - eg indoor flight.
The incorrect innovation consistency check was being applied to the synthetic velocity observations.
This commit is contained in:
Paul Riseborough 2016-01-06 19:58:15 +11:00 committed by Andrew Tridgell
parent a3d781bf3f
commit e80fb8b3fa
1 changed files with 9 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -248,7 +248,7 @@ void NavEKF2_core::FuseVelPosNED()
// 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
// Use different errors if flying without GPS using synthetic position and velocity data
if (PV_AidingMode == AID_NONE && inFlight) {
if (PV_AidingMode == AID_NONE && motorsArmed) {
// 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);
@ -257,6 +257,8 @@ void NavEKF2_core::FuseVelPosNED()
// 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];
R_OBS[5] = posDownObsNoise;
for (uint8_t i=0; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
} else {
if (gpsSpdAccuracy > 0.0f) {
// use GPS receivers reported speed accuracy if available and floor at value set by gps noise parameter
@ -270,14 +272,14 @@ void NavEKF2_core::FuseVelPosNED()
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] = posDownObsNoise;
// 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
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);
for (uint8_t i=3; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
}
R_OBS[5] = posDownObsNoise;
// 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
for (uint8_t i=0; i<=1; i++) R_OBS_DATA_CHECKS[i] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
for (uint8_t i=2; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
// if vertical GPS velocity data and an independant height source is being used, check to see if the GPS vertical velocity and altimeter
// innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting