Information Control Barrier Functions: Preventing Localization Failures in Mobile Systems Through Control

Abstract

This letter develops a new framework for preventing localization failures in mobile systems that estimate their state using measurements. Safety is guaranteed by imposing that the nonlinear least squares optimization, solved in modern localization algorithms, remains well-conditioned. Specifically, the eigenvalues of the Hessian matrix are made to be positive via two methods that leverage control barrier functions to achieve safe set invariance. The proposed method is not constrained to any specific measurement or system type, offering a general solution to the safe mobility with localization problem. The efficacy of the approach is demonstrated on a system provided range-only and heading-only measurements for localization.