New Maneuver System

This is the basic implementation of the new maneuver system. (#1 #2 #16)

It needs more testing and the end points of the maneuvers need explicit headings instead of estimated ones which are prone to round off problems.

Also, acceleration and deceleration are not implemented yet.