Add Vicon position injection. Test with fake from SITL. Need some global parameter for origin lat/lon.