Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 00:04:02 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEK: Reduce settling time after start-up disturbance

Browse Source 
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.
This commit is contained in:
Paul Riseborough 2015-10-22 15:20:41 +11:00 committed by Andrew Tridgell
parent 29aa7001bf
commit 44f42fe72d
1 changed files with 5 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -4032,9 +4032,9 @@ void NavEKF::CovarianceInit()
} }
// quaternions - TODO better maths for initial quaternion covariances that uses roll, pitch and yaw // quaternions - TODO better maths for initial quaternion covariances that uses roll, pitch and yaw
P[0][0] = 1.0e-9f; P[0][0] = 1.0e-9f;
P[1][1] = 0.25f*sq(radians(1.0f)); P[1][1] = 0.25f*sq(radians(10.0f));
P[2][2] = 0.25f*sq(radians(1.0f)); P[2][2] = 0.25f*sq(radians(10.0f));
P[3][3] = 0.25f*sq(radians(1.0f)); P[3][3] = 0.25f*sq(radians(10.0f));
// velocities // velocities
P[4][4] = sq(0.7f); P[4][4] = sq(0.7f);
P[5][5] = P[4][4]; P[5][5] = P[4][4];
@ -4942,12 +4942,8 @@ bool NavEKF::assume_zero_sideslip(void) const
*/ */
float NavEKF::InitialGyroBiasUncertainty(void) const float NavEKF::InitialGyroBiasUncertainty(void) const
{ {
switch (_ahrs->get_ins().get_sample_rate()) { // this is the assumed uncertainty in gyro bias in rad/sec used to initialise the covariance matrix.
case AP_InertialSensor::RATE_50HZ: return 0.035f;
return 1.0f;
default:
return 0.1f;
}
} }
/* /*