bug where bounds on pos angle were -pi to pi. now tentatively works for 4 waypoints
bug where bounds on pos angle were -pi to pi. now tentatively works for 4 waypoints
3 landmark version working