state_to_trajectory function

Position of a state with respect to a trajectory