Improve trajectory ttracking