Update odometry implementation (using world) + test