it works.... not sure what changed. hopefully it's not just for this example
works well except for last waypoint it sometimes doesn't reach the goal
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
keep working on belief version
working on final angle mod bug
believed I fixed angle bug
unable to compile state with -o3, too big. trying on desktop now