AP_NavEKF: Bug fixes to rapid yaw alignment