fixed odom update math errors