AP_NavEKF : Improved Magnetometer Error Handling

(Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and state velocity vector.
(Plane and Copter) Magnetometer fusion now uses corrected data and bias states that are initialised to zero. This allows the compass to be switched in flight.
For persistent compass errors that trigger a timeout, the compass is not permanently failed, however the compass weighting is reduced.