Trajectory analysis in matlab