'Fixed' the math of odometry for now