added trajectory measurement, mode switcher