diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index 94765ea2dd..9be77903f7 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -238,6 +238,7 @@ void NavEKF2_core::InitialiseVariables() runUpdates = false; framesSincePredict = 0; gpsYawResetRequest = false; + stateStruct.quat.initialise(); quatAtLastMagReset = stateStruct.quat; delAngBiasLearned = false; memset(&filterStatus, 0, sizeof(filterStatus));