Better trajectory tracking

[Peque]

• http://idken.net/posts/2018-11-07-trajectory_tracking1/
• http://idken.net/posts/2018-11-07-trajectory_tracking2/