Changing the equations for a correct odometry estimation.