Fix bug in trajectory class