Apply minimal trajectory corrections

[hidmic]

One thing I've noticed is that when the drone destabilizes, it typically does so while trying to maneuver large gate corrections. So it got me thinking: we are required to go through the gates, but not necessarily through their geometrical center. This patch reduces gate corrections to the bare minimum, which arguably increases the risk of collision.

https://user-images.githubusercontent.com/13500507/196007893-79c3cd93-1e04-480a-b8f8-8919da5de783.mp4

[hidmic]

FYI @glpuga

[glpuga]

I like the idea. Same as in the other PR, if it passes the performance test unscathed with:

• mensurable improvement
• no extra collisions

then LGTM.

[hidmic]

This won't do. There are scenarios for which our planner yields sharp turns at nominal gates positions that are somewhat far from their true positions. For those we need the large correction. Arguably, the problem is the planner itself (e.g. a cubic spline would allow us to approach the gate frontally in all cases), but there's not much we can do about it at this point.