Add a 2-waypoint north-south test track.
Add GPS offset compensation to the controller.

Note that the compensation code uses the current timestep IMU data to compensate the GPS, which
isn't quite right. Will need to experiment with using past data here.
Comment out the time consuming part of PlotTrack.
Fix GPS offset computation to do proper rotation.