CHANGE: attempt to fix odometry. Not there yet