The code now controls the robot's motion... although not in a predictable way. The code currently compiles.

I wrote new routines for recording the path history based on odometry.

I need to double check that the way the state vector is initialized is compatible with the way I add the odometry data. I'm wondering if the initial angle isn't set correctly because of this.