other order for grid generation
missing minus for yaw angle clipping
classic atan2 x, y error
more coarse grained grid for testing, also plot goal points