Loop over rectangle points.
2a. Compute heading and distance to point using GPS and great circle geometry.
2b. If within sigma distance of point, advance to next point.
2c. Else use IMU heading to move towards point.
Record all topics to a bag and analyze performance.
Record all topics to a bag and analyze performance.