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.