fixed errors in trajectory integration