Wrote a lot of odometry correction

The position correction works ok. It's hard to tell, since the brick's
speakers don't work and I don't get audible feedback on what it's doing.

I wrote the heading correction, but haven't tried it out yet. Obviously,
there will be problems... Also, I'm not 100% sure the algorithm is good,
since measuring the distance travelled between two recorded gridlines
depends on the odometer's precision (namely the constants for radii, etc.)

In short, whenever the robot passes a grid line, there is always one of the
coordinates that remains unsure.