Update trajectory_interpolation.cpp