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

Consider the nonlinear system

with state $x\in\mathbb{R}^{n}$, control input $u\in\mathbb{R}^{m}$, measured output $y\in\mathbb{R}^{p}$, dynamics $f:\mathbb{R}^{n}\rightarrow\mathbb{R}^{n}$, control input matrix $g:\mathbb{R}^{n}\rightarrow\mathbb{R}^{n\times m}$, and nonlinear measurement model $\mathfrak{m}:\mathbb{R}^{n}\rightarrow\mathbb{R}^{p}$. There are several methods for generating an estimate of the state $x$ using output $y$, e.g., Kalman filtering and state observers. However, recent works in the robotics literature have shown the superiority of optimization-based approaches that involve solving a nonlinear least squares problem in an online fashion [9, 1, 10]. In particular, if measurements $m\in\mathbb{R}^{p}$ are available, then one can use numerical optimization to find the optimal state estimate $\hat{x}^{*}$ by solving

where $\Sigma:\mathbb{R}^{n}\rightarrow\mathcal{S}^{n}$ is a state-dependent positive definite matrix that represents the level of confidence in $m$. We allow $\Sigma$ to be state-dependent as a way to capture measurement degradation/dropout; this will be discussed more in Section IV. If the measurement model does not contain enough information to estimate the full state directly by solving Equation 2, then the nonlinear least squares optimization can be performed over a window of measurements with a motion model. In either case, the proposed approach is applicable. The manifestation of Equation 2 can be traced to nonlinear Maximum A Posteriori (MAP) estimation [1] where maximizing the log likelihood function is more convenient when the likelihood function belongs to the exponential family, i.e., likelihood function is approximately normal. The goal of this work is to ensure the optimization Equation 2 is well-constrained (to be defined later) so a unique state estimate is always available; a guarantee to be achieved through control. In the sequel, we will denote the nonlinear least squares cost as $J(x,{m})$.

This work will employ controlled invariant sets to ensure the optimization Equation 2 always produces a unique estimate $\hat{x}^{*}$. Specifically, set invariance will be achieved with CBFs. The following definitions can be found in many previous works [11, 12] and are stated here for completeness.

This work will also make use of the following definitions from optimization theory, linear algebra, and analysis.

In this section, we present a new approach that prevents the nonlinear least squares optimization defined in Equation 2 from becoming ill-conditioned through the use of CBFs. Central to our approach is the following result that establishes a connection between the uniqueness of a critical point for a twice differentiable function and the eigenvalues of its corresponding Hessian.

Corollary 1 has an important implication for optimization-based localization algorithms: if the Hessian of Equation 2 is degenerate at a critical point, then the computed estimate $\hat{x}^{*}$ may not be unique. From a localization perspective, a non-unique solution to Equation 2 is analogous to elements of the state vector being unobservable; a situation that can have devastating consequences for mobile systems that base decisions and control actions on estimates computed via Equation 2. Consider, e.g., Fig. 1, which shows examples of cost functions that have a non-degenerate and degenerate Hessian in Fig. 1(a) and Fig. 1(b), respectively. The cost function shown in Fig. 1(b) has infinitely many minima that are all valid solutions, any of which can be computed by the localization algorithm and used in feedback. A controller using a non-unique estimate will behave erratically as the estimate is not an accurate representation of the system’s true state, and would likely lead to any number of catastrophic events such state/actuator constraint violations or collisions with obstacles.

Given the discussion above, we define the special class of CBFs that will be the focus for the remainder of this letter.

The minimum eigenvalue of the Hessian of Equation 2 is a measure of how much information about the state is contained in the nonlinear least squares cost function. As such, the class of control barrier functions in Definition 8 ensures that localization information is sufficient for safe operation, i.e., the system’s state can be discerned from the available measurements. A natural choice for a function $h$ to define the safe set in I-CBF is

For Equation 3 to be a valid CBF (based on Definition 4), it must be continuously differentiable and so $\lambda_{\mathrm{min}}(H(x,m))$ must be continuously differentiable. Handling the (potential) non-differentiability of the min operator is fairly straightforward, with approaches presented in [14, 15]. Establishing differentiability of the eigenvalues of the Hessian is more nuanced and requires a thorough investigation. If other degeneracy measures were to be employed instead of Equation 3, e.g., the determinant or condition number of the Hessian, the question of differentiability of the eigenvalues of the Hessian must still be answered. Because of this, proving $\lambda(H)$ to be sufficiently smooth is integral to the proposed method.

The following proposition states an important result for matrices whose eigenvalues are simple, i.e., unique.

If the eigenvalues are non-simple (i.e. they cross¹¹1Interestingly, eigenvalue crossing has been a polarizing topic in quantum mechanics and other related fields, as the question of the so-called avoidance crossing is still debated.) then they still might be differentiable. When the eigenvalues are non-simple, the following theorem can be used to guarantee differentiability but at the expense of stronger assumptions.

Given Propositions 2 and 3, the Hessian of Equation 2 must now be studied to determine if either or when the aforementioned propositions are applicable. The first property that needs to be established is that, for the Hessian $H$ to be continuously differentiable in time, so too must $x$ and $m$. Two assumptions are made to ensure $H$ is continuously differentiable.

With Assumptions 1 and 2, the Hessian $H$ is continuously differentiable in time but, as noted above, this alone is not enough to guarantee differentiability of the eigenvalues everywhere. From Proposition 3, if $H$ depends analytically on a single parameter then the eigenvalues of $H$ are differentiable even in the non-simple case. These two cases lead to two different methods of constructing valid I-CBFs: one where $H$ is analytic and another where the eigenvalues of $H$ are always simple, i.e., the eigenvalues never cross.

The goal of this section is to prove that the eigenvalues of the Hessian $H$ are sufficiently smooth. From Proposition 3, the eigenvalues of $H$ are analytic (and therefore smooth) provided that $H$ itself is an analytic function of only a single parameter. In our framework, this single parameter is time $t$, as the implementation of a controller renders all system dynamics solely time-dependent. To formalize this and prove differentiability for Equation 1, we assume the following.

One of the stronger assumptions made in Theorem 1 is that the control input $u$ is analytic, as it is often computed via a quadratic program for safety-critical control. While conditions under which quadratic programs yield analytic control are discussed in [19], often quadratic programs produce only continuous control. Cohen et al. [20] explore cases where this issue can be mitigated by creating a smooth version of the controller that still satisfies the CBF condition but at the expense of possibly more control effort. One solution discussed in [20] involves modifying the closed-form solution to the quadratic program to make the controller analytic. If the quadratic program is of the form

where $\ell(u,u_{d})$ is convex in $u$, then it admits a closed form solution which can be found using KKT conditions [21]. The closed form solution to Equation 4 is, in general, not analytic as it is often piecewise. For instance, when $\ell(u,u_{d})=\tfrac{1}{2}\|u-u_{d}\|^{2}$, the closed-form solution for $u$ can be expressed as $u=u_{d}+\text{ReLu}(-\Psi)\frac{L_{g}h}{\|L_{g}h\|^{2}}$ where $\Psi=L_{f}h+L_{g}h\,u_{d}+\alpha(h)$, which is not differentiable at $\Psi=0$. However, an arbitrarily close analytic approximation of the ReLu function can be constructed while ensuring the CBF condition is satisfied. For example, one analytic approximation of $u$ is

for $c>1$ always satisfies the conditions for a CBF at the expense of conservatism. The function $\alpha\in\mathcal{K}_{\infty}$ along with parameter $c$ can be adjusted to offset the conservatism.

Having shown the eigenvalues of the Hessian are analytic (and hence smooth) under the conditions needed for Theorem 1, the only remaining item to address is the differentiability of the min operator in Equation 3. We adopt the strategy in [15] that utilizes a smooth under-approximation of the min operator to achieve differentiability without losing set invariance. Specifically, let $\lambda_{i}(x,m)$ be an eigenvalue of the Hessian $H$. The smooth under-approximation to Equation 3 which ensures that the set invariance condition is achieved is $h^{\circ}(x)=-\frac{1}{\kappa}\ln(\sum_{i\in\mathbb{Z}}\text{exp}(-\kappa\,(\lambda_{i}(x,m)-\lambda_{s})))\leq\min_{i\in\mathbb{Z}}\,\lambda_{i}(x,m)-\lambda_{s}=h(x)$, $\kappa\in\mathbb{R}_{+}$. Hence, replacing $h$ with $h^{\circ}$ in the set invariance condition stated in Definition 4 will ensure the minimum eigenvalue of the Hessian never becomes less than (or, as mentioned in Remark 5 greater than) the specified threshold $\lambda_{s}$. To summarize, we smooth the barrier function $h$ from Equation 3 to obtain a differentiable alternative $h^{\circ}$ to circumvent the non-differentiability of the min operator. We then apply Equation 5 to obtain an analytic control input so that the eigenvalues of the Hessian are themselves analytic and therefore differentiable.

Requiring the dynamics and controller to be analytic is needed for the eigenvalues to remain differentiable if the minimum eigenvalue is non-simple. However, if it can be shown the eigenvalues are always simple, i.e., never cross, then the analytic requirement can be eliminated and the differentiability of the eigenvalues is guaranteed. Furthermore, the initial minimum eigenvalue will always be the minimum eigenvalue so the min operator can be removed from Equation 3 and the smoothing under-approximation modification outlined in the previous subsection is no longer necessary. It is therefore desirable to prevent eigenvalue crossing. We now introduce an additional CBF that works in conjunction with an I-CBF to prevent the eigenvalues from crossing. Fundamentally, the anti-crossing CBF maintains a small gap between each pair of eigenvalues ensuring the difference between them is at least ${\delta^{\times}}\in\mathbb{R}_{+}$. This can be imposed by defining the CBF

Since the eigenvalues are now required to not cross, we have $\lambda_{i+1}>\lambda_{i}$ and so $h_{i}^{\times}$ is positive when $\lambda_{i+1}-\lambda_{i}>{\delta^{\times}}$ thereby defining the desired super-level set. There are several alternatives to Equation 6 but the advantage of our choice is its simplicity and effectiveness (as was also the case for Equation 3).

To show Equation 6 is minimally invasive, we leverage the following result that provides insights about the structure of the manifold on which the eigenvalues could cross.

The anti-crossing CBFs $h_{i}^{\times}$ are minimally invasive as eigenvalues of the Hessian can only cross on a manifold of size $n-2$ where $n$ is the size of the Hessian as stated in Proposition 4. This property implies that the system’s trajectory will avoid this manifold without significantly deviating from the desired control input $u_{d}$ or the desired state $x_{d}$, as it can circumvent the lower dimensional manifold with minimal change. For instance, if $n=3$, the points to avoid are one-dimensional, meaning that the trajectory must avoid specific lines in three-dimensional space.

Regarding implementation, the analytic method requires stronger assumptions (Assumption 3) but can be computationally faster as seen in Section IV while the anti-crossing method is more intuitive and uses fewer hyperparameters.

A subtle but important element of the proposed approach is the need to have information regarding the rate of change of measurements with respect to time. In other words, the approach utilizes a form of measurement prediction to achieve set invariance in a similar manner to how the system’s dynamics are used. For the sake of clarity, let $\lambda_{\text{min}}$ be simple. Then, from Equation 3 and Definition 8, $\dot{h}(x,m)=\frac{\partial h}{\partial x}\dot{x}+\frac{\partial h}{\partial m}\dot{m}\geq-\alpha(h(x,m))$ where the time variation of the measurements $m$ must be accounted for to achieve set invariance. Intuitively, the derivative can be thought of as providing an instantaneous prediction of the next measurement. In practice, one rarely has access to $\dot{m}$, so it must be estimated directly or approximated via $\mathfrak{m}(x)$. Future works will investigate this trade-off, but we expect the latter—which we call using predictive measurement models—will be a fruitful research area given the various sensing modalities, such as vision, LiDAR, event camera, etc., used on mobile systems. This work will utilize the approximation $m\approx\mathfrak{m}(x)$.

The previous analysis can be extended to control barrier functions that have a relative degree greater than one. Various approaches for constructing CBFs suitable for higher relative-degree systems have been suggested in the literature [23, 24, 25]. Among these, one method that works well for I-CBFs is that presented in [24]. In this method, a new CBF $h_{r}$ is constructed with a relative degree of one that in the limit has the same safe set as the original $h$. This new $h_{r}$ is then an implementable I-CBF. In the previous analysis, if the relative degree for the original $h$ is $r>1$, Assumption 2 needs to be modified to $r$-times continuously differentiable, to ensure that the eigenvalues are sufficiently smooth. This method for higher relative degrees works for both the analytic Hessian and anti-crossing CBF, as is demonstrated in Section IV.

The measurement model for range-only beacons is $\mathfrak{m}_{k}(p_{x},p_{y})=\sqrt{(p_{x}-b^{k}_{x})^{2}+(p_{y}-b^{k}_{y})^{2}}$ for $k=1,2,3$. The covariance matrix $\Sigma(p_{x},p_{y})$ for each measurement was chosen to be $\Sigma(p_{x},p_{y})_{k,k}={1+\exp({m_{k}(p_{x},p_{y})-10})}$ to capture measurement dropout due to distance from the $k^{\text{th}}$ beacon. The off-diagonal entries were set to zero.

Localization. For localization, the safe region where $\lambda_{s}\geq 5$ is shown as the region inside the red line in Fig. 2(a) with the grey scale that denotes the value of $\lambda_{\text{min}}$ as a function of $p_{x}$ and $p_{y}$. Both the analytic and anti-crossing methods demonstrated good performance in Fig. 2(c), ensuring that $h$ remained positive and that the minimum eigenvalue stayed above the specified threshold. The anti-crossing method was more aggressive as the system took a more direct path to the goal and subsequently got closer to the boundary of $\mathcal{S}$. Moreover, the max control effort was twice as large in this method compared with the analytic I-CBF, highlighting the trade-off between the analytic and anti-crossing formulations. The average per-step computation time was 0.7 ms for the analytic method and 35.6 ms for the anti-crossing method. The majority of the computation time for the anti-crossing method came from the optimization while formulating the Hessian and its derivatives took only 0.08 ms.

Detection Avoidance. As noted in Remark 5 the method in this letter can also be used for detection avoidance by ensuring that the minimum eigenvalue of the hessian remains sufficiently small. The safe set where detection can be avoided is $\lambda_{s}\leq 5$ and is indicated by the red line in Fig. 2(b). The system remained outside of the detectable set as the value of the barrier function remained positive Fig. 2(d). Like with the localization, the anti-crossing method was more aggressive as compared to the analytic method.

Next, we demonstrate the method using bearing-only measurements for localization. The measurement model is $\mathfrak{m}_{k}(p_{x},p_{y})=\tan^{-1}\left((p_{y}-b^{k}_{y})/(p_{x}-b^{k}_{x})\right)$ for $k=1,2,3$ and the weighting matrix is $\Sigma(p_{x},p_{y})=I$ since the eigenvalues naturally decayed as the distance from the beacons increased thereby eliminating the need to model measurement degradation/dropout separately.

Localization. For safe localization the safe region, where $\lambda_{s}\geq 0.01$ is shown in Fig. 3(a) by the red line. Looking at Fig. 3(c), both the analytic and anti-crossing methods maintained $h$ above zero, but their values deviated substantially. As with the range-only measurements, the analytic method was more conservative, but the peak control effort was approximately five times less. The average per-step computation time for the analytic and anti-crossing methods was 0.8 ms and 19.4 ms respectively, in the anti-crossing computing the Hessian and its derivatives took only 0.06 ms. Both methods ensured the eigenvalue of the Hessian did not drop below the specified threshold, even with the complex shape of the safe set $S$.

Detection Avoidance. The bearing-only measurements can also be used in a detection avoidance formulation. For detection avoidance, the safe region is where $\lambda_{s}\leq 0.01$ is shown in Fig. 3(b) by the red line. Both the anti-crossing method and the analytic methods kept the system in the safe set as indicated by the value of the barrier function Fig. 3(d) with the anti-crossing exhibiting more aggressive behavior.