Matlab Trajectory Code