AP_NavEKF2: Remove unnecessary variance reset

This reset is unnecessary give that a synthetic yaw angle is fused at a low rate to prevent variances from becoming badly conditioned.
This commit is contained in:
Paul Riseborough 2016-05-21 11:47:07 +10:00 committed by Andrew Tridgell
parent 1179c08473
commit e3c966661b
1 changed files with 0 additions and 5 deletions

View File

@ -124,11 +124,6 @@ void NavEKF2_core::realignYawGPS()
// reset the magnetometer field states - we could have got bad external interference when initialising on-ground
calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
// if we are not using a magnetometer, then the yaw gyro bias value will be invalid at this point and the
// state variance should be reset
if (!use_compass()) {
P[11][11] = sq(radians(InitialGyroBiasUncertainty() * dtEkfAvg));
}
// We shoud retry the primary magnetometer if previously switched or failed
magSelectIndex = 0;