Removed failure case checking (makes errors worse when odometry fails)