AP_NavEKF : Cleaned up InitialiseFilterDynamic