Fixed mistake in odometry code