Fix correct usage of angular velocity in update_odometry() function

Fix correct usage of angular velocity in update_odometry() function #1549