improve trajectory planning