Cleaned up a little bit and fixed odometry errors.