Implementation of efficient algorithms for globally optimal trajectoriesL. PolymenakosD.P. Bertsekaset al.1998IEEE TACON