From 92c30260728d92ee4be8d08473e9068ef1c54926 Mon Sep 17 00:00:00 2001 From: priseborough Date: Sun, 21 Dec 2014 21:14:53 +1100 Subject: [PATCH] AP_NavEKF: Link process noise to arm status, not pos hold mode --- libraries/AP_NavEKF/AP_NavEKF.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 06a42933da..f67fdf5dd8 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -1213,10 +1213,10 @@ void NavEKF::CovariancePrediction() } for (uint8_t i= 0; i<=9; i++) processNoise[i] = 1.0e-9f; for (uint8_t i=10; i<=12; i++) processNoise[i] = dAngBiasSigma; - // scale gyro bias noise when in position hold mode to allow for faster bias estimation + // scale gyro bias noise when disarmed to allow for faster bias estimation for (uint8_t i=10; i<=12; i++) { processNoise[i] = dAngBiasSigma; - if (posHoldMode) { + if (!vehicleArmed) { processNoise[i] *= _gyroBiasNoiseScaler; } }