Update trajectory_interpolation.h