Fixed error in iterative EKF break condition