Added odometry calculations.  Not meant to compile just wanted to get my thoughts down before Thursday.