Add initial SLAM implementation.

So far, I've tested the odometry part and it seems to work
pretty well. The next step, of course, is testing the Kalman
filter.