From 27393855f116c2cfba9a07d8259e637e4c85e9e4 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 15 Oct 2015 10:20:55 +1100 Subject: [PATCH] AP_NavEKF2: Change assumed gyro calibration accuracy If an external gyro calibration has been performed, we should assume that it has been done under static conditions Otherwise it is pointless and we should allow the EKF to find its own gyro bias offsets. --- libraries/AP_NavEKF2/AP_NavEKF_GyroBias.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF_GyroBias.cpp b/libraries/AP_NavEKF2/AP_NavEKF_GyroBias.cpp index 847670920e..16abae685f 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF_GyroBias.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF_GyroBias.cpp @@ -19,12 +19,15 @@ extern const AP_HAL::HAL& hal; // reset the body axis gyro bias states to zero and re-initialise the corresponding covariances +// Assume that the calibration is performed to an accuracy of 0.5 deg/sec which will require averaging under static conditions +// WARNING - a non-blocking calibration method must be used void NavEKF2_core::resetGyroBias(void) { stateStruct.gyro_bias.zero(); zeroRows(P,9,11); zeroCols(P,9,11); - P[9][9] = sq(radians(InitialGyroBiasUncertainty() * dtIMUavg)); + + P[9][9] = sq(radians(0.5f * dtIMUavg)); P[10][10] = P[9][9]; P[11][11] = P[9][9]; } @@ -38,4 +41,4 @@ float NavEKF2_core::InitialGyroBiasUncertainty(void) const } -#endif // HAL_CPU_CLASS \ No newline at end of file +#endif // HAL_CPU_CLASS