AP_NavEKF : Improved heading and magnetic field state initialisation