From 44f42fe72d9a752a7e4596b4990872950545854d Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 22 Oct 2015 15:20:41 +1100 Subject: [PATCH] AP_NavEK: Reduce settling time after start-up disturbance Now that we are using a consistent 50Hz minimum update rate for the covariance prediction we do not need a different initial gyro bias uncertainty for plane and copter to maintain filter stability margins. The default value of 0.1 rad/s was too high and gave excessive settling time of the filter attitude after startup. The initial attitude uncertainty has been increased to allow for some movement during startup. --- libraries/AP_NavEKF/AP_NavEKF.cpp | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 33b52699cf..30fbb58729 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -4032,9 +4032,9 @@ void NavEKF::CovarianceInit() } // quaternions - TODO better maths for initial quaternion covariances that uses roll, pitch and yaw P[0][0] = 1.0e-9f; - P[1][1] = 0.25f*sq(radians(1.0f)); - P[2][2] = 0.25f*sq(radians(1.0f)); - P[3][3] = 0.25f*sq(radians(1.0f)); + P[1][1] = 0.25f*sq(radians(10.0f)); + P[2][2] = 0.25f*sq(radians(10.0f)); + P[3][3] = 0.25f*sq(radians(10.0f)); // velocities P[4][4] = sq(0.7f); P[5][5] = P[4][4]; @@ -4942,12 +4942,8 @@ bool NavEKF::assume_zero_sideslip(void) const */ float NavEKF::InitialGyroBiasUncertainty(void) const { - switch (_ahrs->get_ins().get_sample_rate()) { - case AP_InertialSensor::RATE_50HZ: - return 1.0f; - default: - return 0.1f; - } + // this is the assumed uncertainty in gyro bias in rad/sec used to initialise the covariance matrix. + return 0.035f; } /*