# A Control Approach for Nonlinear Stochastic State Uncertain Systems with
Probabilistic Safety Guarantees

Mohammad S. Ramadan, Mohammad Alsuwaidan, Ahmed Atallah, and Sylvia Herbert
The authors are with the Department of Mechanical and Aerospace Engineering,
University of California San Diego, La Jolla, CA, USA. E-mails: {msramada,
mnalsuwa, aatallah<EMAIL_ADDRESS>
###### Abstract

This paper presents an algorithm to apply nonlinear control design approaches
in the case of stochastic systems with partial state observation.
Deterministic nonlinear control approaches are formulated under the assumption
of full state access and, often, relative degree one. We propose a control
design approach that first generates a control policy for nonlinear
deterministic models with full state observation. The resulting control policy
is then used to build an importance-like probability distribution over the
space of control sequences which are to be evaluated for the true stochastic
and state-uncertain dynamics. This distribution serves in the sampling step
within a random search control optimization procedure, to focus the
exploration effort on certain regions of the control space. The sampled
control sequences are assigned costs determined by a prescribed finite-horizon
performance and safety measure, which is based on the stochastic dynamics.
This sampling algorithm is parallelizable and shown to have computational
complexity indifferent to the state dimension, and to be able to guarantee
safety over the prescribed prediction horizon. A numerical simulation is
provided to test the applicability and effectiveness of the presented approach
and compare it to a certainty equivalence controller.

##  I Introduction

The challenge with output feedback control in stochastic systems is the
interaction between control and estimation. Stochastic control deals with this
interaction by introducing the concept of the information state [12, 19],
which addresses the propagation of uncertainty together with the state
estimate, instead of regarding this estimate as the true state value as in
certainty equivalence control. The introduction of the information state
results in a significant increase in the complexity and dimensionality of the
control problem and leads to algorithms which are prohibitive computationally.
On the other hand, to maintain computational tractability, nonlinear control
design approaches are typically formulated for deterministic and full-state
feedback systems [11]. This difference in the problem formulation makes it
difficult to use nonlinear design approaches in stochastic settings.

In this paper, we propose a random search algorithm to solve the stochastic
nonlinear optimal control problem. For computational efficiency, we first
employ a nonlinear controller designed for a surrogate deterministic dynamic
model. The nonlinear controller and the surrogate dynamics are used as a
constructor of an importance-like distribution which reduces the searching
space and guides the effort of sampling finite-horizon control sequences.
These sequences are then evaluated with respect to a prescribed finite-horizon
performance and safety measure when applied to the true stochastic dynamics.
The first control action, of the sequence with the highest performance and
safety measure is applied at the current time in a receding-horizon fashion.

Out of the many nonlinear design approaches to test the proposed algorithm,
this paper uses control barrier functions (CBF) to generate the deterministic
control policy. CBFs are considered the “dual” of control Lyapunov functions,
but they enforce safety rather than stability. Existing literature about CBFs
has limited answers to system uncertainties or output feedback, two
characteristics that often exist in real-world systems. A method addressing
stochastic systems within the CBF framework have been developed in [7], but
for the full state-feedback case. For the case of partial state observation,
or output feedback, the CBF-based approaches are typically limited in
assumptions to linear systems [20]. Furthermore, time-discretization, and
possibly state and input space discretization, are employed while relying on
guarantees achieved in continuous settings. This weakens the validity of the
inherited guarantees and leads to the conservativeness in the adjusting safe
sets by additional safety margins [3].

The key idea of this paper is to use the policy generated by the deterministic
CBF controller to guide a random search optimization for the stochastic
nonlinear control problem. This bears resemblance to importance sampling, an
important technique in the field of Monte Carlo integration [13]. This
technique approximates an integral by sampling the importance distribution: a
chosen probability distribution that is ideally concentrated in regions of
high contribution to the integral in-hand. The analogy between our proposed
algorithm, and importance distribution, is in constructing a distribution in
the control space that is concentrated in regions likely to be of low cost and
guaranteed safety. This is similar to sampling methods which rely on an
information theoretic construction of the importance distribution [21] for
full state-feedback systems. However, our approach does not require full
state-feedback. Moreover, in contrast to sampling approaches based on Monte
Carlo integration, our approach does not rely on the laws of large numbers; a
finite number of samples/scenarios is enough to guarantee safety with high
probability.

##  II Problem Formulation

Consider the following discrete-time state-space model

$\displaystyle x_{k+1}$ $\displaystyle=f(x_{k},u_{k},w_{k}),$ (1a)
$\displaystyle y_{k}$ $\displaystyle=g(x_{k},v_{k}),$ (1b)

where the state is $x_{k}\in\mathbb{R}^{r_{x}}$, control input
$u_{k}\in\mathbb{R}^{r_{u}}$, and exogenous disturbances $w_{k}$ and $v_{k}$.
The initial state $x_{0}$ has distribution $p_{0}$ represented by a set of $L$
equiprobable particles $\Xi=\\{x_{0\mid 0,j}\mid j=1,\ldots,L\\}$. The
stochastic disturbance processes, $\\{w_{k}\\}$ and $\\{v_{k}\\}$, are white,
possess known densities $\mathcal{W}$ and $\mathcal{V}$, and are independent
from each other and each from $x_{0}$.

We assume we are given the following cost function

$\displaystyle
J(p_{0},\\{u_{k}\\}_{k=0}^{N-1})=\mathbb{E\,}\left(\gamma^{N}\ell_{N}(x_{N})+\sum_{k=0}^{N-1}\gamma^{k}\ell_{k}(x_{k},u_{k})\right),$

then, the objective is to find the control sequence $\\{u_{k}\\}_{k}^{N-1}$
that solves the following optimization problem:

$\displaystyle\min_{\\{u_{k}\\}_{k=0}^{N-1}}$ $\displaystyle
J(p_{0},\\{u_{k}\\}_{k=0}^{N-1}),$ (2) $\displaystyle\text{subject to, for all
}k,$ $\displaystyle\mathbb{P}(x_{k}\in\mathcal{C})\geq
1-\epsilon,\,u_{k}\in\mathbb{U},$

where: $\gamma\in(0,1]$ is the discount factor; $N$ is the prediction horizon;
the expectation $\mathbb{E\,}$ is with respect to a probability space of:
elements $(x_{0},\\{w_{k}\\}_{k=0}^{N-1})$, the suitable product Borel
$\sigma-$field, and the probability measure $\mathbb{P}$; $\ell_{k}$s are the
stage cost functions and
$\mathbb{E\,}\lvert\ell_{k}(x_{k},u_{k})\rvert<\infty$111Possible assumptions
to satisfy this: $\ell_{k}$s are bounded; or for each $u_{k}$,
$\ell_{k}(\cdot,u_{k})$ is positive and bounded by a quadratic function in
$x_{k}$, the covariances of $w_{k}$, $x_{0}$ are finite and $f$ is uniformly
Lipschitz., for all $u_{k}$; $\epsilon\in[0,1)$ is the acceptable constraint
violation probability and is typically small $\epsilon\approx 0$; the set
$\mathcal{C}$ can be seen as the safe set and is not to be violated in a
probability less than $1-\epsilon$.

In the case of partial state observation as in (1b), where in general
$y_{k}\neq x_{k}$, the concept of the information state appears [12], which is
the conditional state density. Designing a control law over the function of
all possible densities was concluded very early in the control literature to
be prohibitive [19]. One of the common approaches is to ignore the state
uncertainty and use an estimate of the state as the true state in a feedback
control law. This approach is valid only in simple examples like linear-
quadratic-Gaussian (LQG) control, where the separation principle holds [2]. In
this paper, our approach to solve (2) takes into account the state uncertainty
and the disturbances, and provides probabilistic guarantees for the original
system (1).

##  III Prerequisites

Our methodology consists of two major components: A CBF controller (for the
surrogate deterministic dynamics) and a particle filter to track the state
conditional density. This section gives a brief introduction of each, before
we introduce the algorithm in the next section.

###  III-A Control Barrier Functions

Although the dynamics used in the CBF and safety literature are mostly in
continuous-time, the discrete-time dynamics can be obtained by some integrator
or simply a zero-order-hold. Thus, we first assume deterministic dynamics and
full state accessibility to derive a CBF controller through a surrogate
continuous-time formulation. Then we apply a zero-order-hold, accounting for
partial observations and exogenous disturbances, to get back the formulation
(1).

The following continuous-time nonlinear dynamic system conforms to assumptions
concerning the existence and uniqueness of its solutions,

$\displaystyle\dot{x}=F(x,u),$ (3)

where $x\in\mathbb{R}^{r_{x}}$ is the state and
$u\in\mathbb{U}\subset\mathbb{R}^{r_{u}}$ is the control input. This system is
to be made safe, in the sense of remaining in a control invariant set
$\mathcal{C}\subset\mathbb{R}^{r_{x}}$ that is a subset of the state
constraints.

Control barrier functions are real-valued functions over the state space that
encode both the state constraints and long-term effects of the dynamic system.
A CBF has two key properties relevant to safety: its value at a given state
provides a measure of safety, and its gradient informs the set of control
actions that will preserve safety [1, 6]. Let $h(x)$ be a smooth real-valued
function such that $\mathcal{C}$ is the superlevel set of $h$, that is,
$\mathcal{C}=\\{x\in\mathbb{R}^{r_{x}}\mid h(x)\geq 0\\}$, and let
$\alpha:\mathbb{R}\to\mathbb{R}$ be a class $\mathcal{K}$ function [11, p.
144]. Then, the function $h$ is called a CBF if

$\displaystyle\exists u\in\mathbb{U}\text{ such that }\frac{\partial
h}{\partial x}\cdot F(x,u)=\dot{h}\geq-\alpha(h),\,\forall x.$ (4)

The above condition enforces that the system remains within the control
invariant set $\mathcal{C}$, and therefore preserves non-negative safety
values.

The CBF is typically paired with a background performance controller that is
designed according to a different safety-blind objective [4, 6]. Online, the
CBF acts as a safety filter to minimally modifies the performance control in
order to maintain safety:

$\displaystyle u^{\star}(x)=\operatorname*{arg\,min}_{u\in\mathbb{U}}\,\lVert
u-u_{0}\rVert^{2},$ (5) $\displaystyle u\text{ satisfies the inequality in
\eqref{eq:CBF_condition}}.$

The variable $u_{0}$ represents the control value chosen by this background
controller, and $u^{\star}$ is the modified safe control. If the dynamic model
(3) is affine in $u$, (5) is a quadratic program and can be efficiently solved
online.

###  III-B Particle filtering

For general nonlinear dynamics of the form (1), the particle filter is a
convenient approximation to the Bayesian filter [8]. We continue assuming, as
in (1), that the current time ($k=0$) state filtered density $p_{0}$ is
provided by a set of equiprobable particles
$\Xi=\\{\xi^{j}_{0}\in\mathbb{R}^{r_{x}},j=1,\dots,L\\}$. The future times’
particles are then achieved by the particle filter algorithm.

The dynamics in (1) have an equivalent representation using transition and
measurement densities [18]: $p(x_{k+1}\mid x_{k},u_{k})$ and $p(y_{k}\mid
x_{k})$222The notation $p(a\mid b,c)$ denotes the density function of $a$
given the values $b$ and $c$., respectively. These densities are characterized
by $\mathcal{V}$ and $\mathcal{W}$, and the dynamics $f$ and $g$ in (1).

The initial state density $p_{0}$ is defined by the particle set
$\Xi=\\{x_{0\mid 0,j}\mid j=1,\ldots,L\\}$, which is assumed to be given. The
two major steps of a basic (bootstrap) particle filter are

  1. 1.

The time update step, in which the set of particles
$\\{x_{k|k,j}\\}_{j=1}^{L}$ are propagated through the state equation (1a),
with a chosen $u_{k}$, using $L$ realisations of the disturbance
$w_{k,j}\sim\mathcal{W}$. The resulting particles are denoted
$\\{x_{k+1|k,j}\\}_{j=1}^{L}$.

  2. 2.

The measurement update step, wherein if $y_{k+1}$ becomes available, consists
of computing the importance weights $\\{\Omega_{k+1,j}\\}_{j=1}^{L}$ by:

$\Omega_{k+1,j}=\frac{p(y_{k+1}\mid x_{k+1|k,j})}{\sum_{j=1}^{L}p(y_{k+1}\mid
x_{k+1|k,j})},\,j=1,\ldots,L.$

In this paper, we implement a resampling step after Step-(2), for every time
step. The resulting equiprobable particles are denoted $\\{x_{k+1\mid
k+1}\\}_{j=1}^{L}$. The conditional mean at any time $k$ can be found by
evaluating the sample average

$\mathbb{E\,}[x_{k}\mid
y_{0},\ldots,y_{k}]\approx\frac{1}{L}\sum_{j=1}^{L}x_{k|k,j}.$

##  IV Control importance-like distribution for random search

Our random search approach to solve (2) is influenced by importance sampling
in Monte Carlo literature, which prioritizes sampling regions in the
integration space of higher contribution. We construct an importance-like
distribution $\mathcal{U}$ of control sequences $\\{u_{k}\\}_{k=0}^{N-1}$,
that is ideally concentrated at regions with high confidence to contain
feasible, and possibly optimal solutions of (2).

Sampling the distribution $\mathcal{U}$ is done through a simulation-based
algorithm. We first simulate the sequence $\\{x_{k}^{\prime}\\}_{k}$
characterized by an initial state $x_{0}\sim p_{0}$ and a CBF controller
applied in feedback $u^{\star}_{k}=\pi(x_{k})$ obtained from (5), in

$\displaystyle
x_{k+1}^{\prime}=f(x_{k}^{\prime},\pi(x_{k}^{\prime}),w_{k}^{\prime}),\,x_{0}^{\prime}\sim
p_{0},\,w_{k}^{\prime}\sim\mathcal{W},$ (6)

where the disturbance $\\{w_{k}^{\prime}\\}_{k}$ is an i.i.d. realization of
$\\{w_{k}\\}_{k}$.

This sampling procedure defines a distribution of the state sequence
$\\{x_{k}^{\prime}\\}_{k=0}^{N-1}$ which in turn defines a distribution of the
control sequence
$\\{u_{k}^{\star}\\}_{k=0}^{N-1}=\\{\pi(x_{k}^{\prime})\\}_{k=0}^{N-1}$. We
call this the importance distribution $\mathcal{U}$. Notice that this
distribution is a function of: the original dynamics (1a), the control law
$\pi(\cdot)$ (which is based on the chosen surrogate dynamics (3)), and the
initial state density $p_{0}$. Our Control Importance Distribution Algorithm
(CIDA), shown in Algorithm 1, outlines333The function $1(\cdot)$, used in
Algorithm 1, is the indicator function and returns $1$ if $\cdot$ is true and
$0$ if false. this random search procedure we propose to solve (2).

Algorithm 1 Control Importance Distribution Algorithm (CIDA)

Input

$p_{0}$ current time, $k=0$, particle filter’s density described by
equiprobable particles $\Xi$
---

$\pi$ CBF safe controller (5) of the surrogate deterministic dynamics defined
in (3)
---

Mnumber of simulations per control sequence

$\alpha$acceptable statistical violation rate, $\alpha\in[0,\epsilon)$

R rollouts number, i.e. the number of control sequence random search trials
---

Output

$u^{\\#}$current time control input

for $i=1,2,\ldots,R$ do

Using $p_{0}$ and $\pi$, sample $\\{x_{k}^{\prime}\\}_{k=0}^{N-1}$ using (6),
then record
$\\{u_{k}^{\star}\\}_{k=0}^{N-1}=\\{\pi(x_{k}^{\prime})\\}_{k=0}^{N-1}$;
---

using $\\{u_{k}^{\star}\\}_{k=0}^{N-1}$, $M-$(iid) simulations of
$\\{w_{k}\\}_{k=0}^{N-1}$, denoted $(\\{w_{k,q}\\}_{k=0}^{N-1})_{q=1}^{M}$,
and the state equation (1a), record the $M-$trajectories
$(\\{x_{k,q}\\}_{k=0}^{N})_{q=1}^{M}$ such that
---
$\displaystyle x_{k+1,q}$ $\displaystyle=f(x_{k,q},u_{k}^{\star},w_{k,q});$

if $\displaystyle a_{k}=\frac{1}{M}\sum_{q=1}^{M}1(x_{k,q}\in\mathcal{C})\geq
1-\alpha,\forall k$ (7) then

$\\{u_{k}^{\star}\\}_{k=0}^{N-1}$ is feasible, and calculate $J_{i}$
$\displaystyle
J_{i}=\frac{1}{M}\sum_{q=1}^{M}\left(\gamma^{N}\ell_{N}(x_{N,q})+\sum_{k=0}^{N-1}\gamma^{k}\ell_{k}(x_{k,q},u_{k}^{\star})\right);$

else

$\\{u_{k}^{\star}\\}_{k=0}^{N-1}$ is infeasible, and
$\,J_{i}\leftarrow\infty$;

end if

end for

If finite, find $\min_{i}J_{i}$ and its corresponding control sequence
$\\{u_{k}^{\star}\\}_{k=0}^{N-1}$, then set $u^{\\#}\leftarrow u_{0}^{\star}$;

Propagate particle filter to next time and reset $k\leftarrow 0$;

###  IV-A Computational complexity of CIDA

Notice that in the original problem formulation (2), the safety condition
$\\{x_{k}\in\mathcal{C}\\}$ has to be satisfied with probability $\geq
1-\epsilon$. However, Algorithm 1 imposes the safety condition statistically
for the $M-$samples, and with rate $\geq 1-\alpha$. Relying only on the laws
of large numbers for guarantees demands $M\to\infty$. Similar to [17, 14], and
the randomized sample/scenario framework [5], we can provide acceptable safety
guarantees with relatively small444In the numerical simulations section we use
$M=150$. number of samples.

######  Lemma 1.

(Hoeffding’s inequality [10]). For independent random variables
$Z_{q},\,q=1,\ldots,\bar{M}$, $\mathbb{P}(Z_{q}\in[a_{q},b_{q}])=1$,
$a_{q}\leq b_{q}$, for all $t\geq 0$

$\displaystyle\mathbb{P}\left(\sum_{q=1}^{\bar{M}}(Z_{q}-\mathbb{E\,}\,Z_{q})\geq
t\bar{M}\right)\leq\exp\left(-\frac{2\bar{M}^{2}t^{2}}{\sum_{q=1}^{\bar{M}}(b_{q}-a_{q})^{2}}\right)$

∎

######  Theorem 1.

For any $\delta\in(0,1)$, if

$\displaystyle
M\geq\frac{1}{2(\epsilon-\alpha)^{2}}\log\left(\frac{1}{\delta}\right),$ (8)

then, a feasible control sequence $\\{u_{k}\\}_{k=0}^{N-1}$ w.r.t. Algorithm
1, i.e. satisfies the inequalities (7), is feasible with respect to the
original optimization problem (2), with probability $1-\delta$.

###### Proof.

Given an initial density $p_{0}$, fix a control sequence
$\\{u_{k}\\}_{k=0}^{N-1}$, where $u_{k}\in\mathbb{U}$, and let
$\\{x_{k}\\}_{k=0}^{N}$ be the resulting process characterized by (1a). Let
$(\\{x_{k,q}\\}_{k=0}^{N})_{q=1}^{M}$ be defined as in Algorithm 1, using this
$\\{u_{k}\\}_{k=0}^{N-1}$.

Suppose $\\{u_{k}\\}_{k=0}^{N-1}$ is infeasible with respect to (2). Then
$\exists m\in\\{1,\ldots,N\\}$, a minimizer of
$G_{k}=\mathbb{P}(x_{k}\in\mathcal{C})$ and hence
$G_{m}=\mathbb{P}(x_{m}\in\mathcal{C})<1-\epsilon$. Define
$Y_{q}={1}(x_{l,q}\in\mathcal{C})$. Then

$\displaystyle\mathbb{E\,}Y_{q}=\mathbb{P}(x_{l,q}\in\mathcal{C})=\mathbb{P}(x_{l}\in\mathcal{C})<1-\epsilon$

Since $x_{l,q}$ is an iid sample of $x_{l}$.

Now we check the statistical feasibility of this control sequence, according
to Algorithm 1,

$\displaystyle\mathbb{P}\left(\left\\{\frac{1}{M}\sum_{q=1}^{M}{1}(x_{k,q}\in\mathcal{C})\geq
1-\alpha,\,\forall k\right\\}\right),$
$\displaystyle\leq\mathbb{P}\left(\left\\{\frac{1}{M}\sum_{q=1}^{M}{1}(x_{l,q}\in\mathcal{C})\geq
1-\alpha\right\\}\right),$
$\displaystyle\leq\mathbb{P}\left(\frac{1}{M}\sum_{q=1}^{M}\left(Y_{q}-\mathbb{E\,}Y_{q}\right)\geq-1+\epsilon+1-\alpha\right),$
$\displaystyle\leq\mathbb{P}\left(\sum_{j=1}^{M}\left(Y_{q}-\mathbb{E\,}Y_{q}\right)\geq
M(\epsilon-\alpha)\right),$
$\displaystyle\leq\exp\left(-2M(\epsilon-\alpha)^{2}\right),$

where the second inequality follows from the definition of $Y_{q}$, and the
last inequality from Hoeffding’s (Lemma 1). If we pick $\delta\in(0,1)$, and
pick an $M$ that satisfies (8) with strict inequality, we get

$\displaystyle\mathbb{P}\left(\left\\{\frac{1}{M}\sum_{q=1}^{M}{1}(x_{k,q}\in\mathcal{C})\geq
1-\alpha,\,\forall k\right\\}\right),$
$\displaystyle\leq\exp\left(-2M(\epsilon-\alpha)^{2}\right)<\delta.$

Therefore, if $\\{u_{k}\\}_{k=0}^{N-1}$ is infeasible w.r.t. (2), then it is
infeasible w.r.t. Algorithm 1, with probability $\geq 1-\delta$. The
contrapositive of this statement proves this theorem. ∎

Algorithm 1 has a computational complexity of $\mathcal{O}(MR)$, per time-
step. By choosing $R$ to be of $\mathcal{O}(L)$, where $L$ represents the
number of particles in the particle filter, the overall complexity of the
algorithm, combined with the particle filter, becomes $\mathcal{O}(L\log L)$
per time-step. This algorithm is parallelizable across two dimensions: $i$,
the random search trial number; and $q$, the index of the $M$ resulting
trajectories. Using a graphics processing unit (GPU) can potentially reduce
the required computation time.

######  Remark 1.

If $x_{0}^{\prime}=\mathbb{E\,}x_{0}$ and $w_{k}^{\prime}=0$ are used in (6),
the resulting control sequence, call it $\\{\bar{u}_{k}\\}_{k}$, is the
certainty equivalence control [2]. Simply augmenting this sequence as one of
the rollouts in Algorithm 1 guarantees that this algorithm is as good or
better, in performance and safety over the prediction horizon, than the
typically used certainty equivalence control.

######  Remark 2.

The distribution of $w_{k}^{\prime}$ determines the “breadth” of the searching
space around the certainty equivalence control. Although $w_{k}^{\prime}$ is
presented as an i.i.d. sample of $w_{k}$, it can be sampled according to a
different distribution. When there is no modeling mismatch: i.e. $w_{k}=0$ and
$x_{0}$ is known, we expect the certainty equivalence control (with
$w_{k}^{\prime}=0$) to be optimal w.r.t. (2). The more the uncertainties and
modeling mismatches, the more the optimal solution will possibly depart from
the certainty equivalence one, the more breadth we need for the distribution
of $w_{k}^{\prime}$, such that the optimal solution is still in the support of
$\mathcal{U}$.

##  V Numerical simulation: autonomous vehicle navigation and obstacle
avoidance

An autonomous vehicle is modelled as a discrete-time stochastic unicycle
model, with partial state observation:

$\displaystyle\xi_{k+1}$ $\displaystyle=f(\xi_{k},\omega_{k},q_{k}),$
$\displaystyle=\xi_{k}+\tau\begin{bmatrix}V\operatorname{sinc}(\frac{\omega_{k}\tau}{2})\cos(\theta_{k}+\frac{\omega_{k}\tau}{2})\\\
V\operatorname{sinc}(\frac{\omega_{k}\tau}{2})\sin(\theta_{k}+\frac{\omega_{k}\tau}{2})\\\
\omega_{k}\end{bmatrix}+w_{k},$ (9) $\displaystyle z_{k}$
$\displaystyle=g(\xi_{k},v_{k})=\begin{bmatrix}x_{k}\\\
y_{k}\end{bmatrix}+v_{k},$ (10)

where $\tau=0.2\,s$ is the time interval of one step,
$\operatorname{sinc}(\cdot)=\sin(\cdot)/\cdot$ is the sinc function,
$\xi_{k}=(x_{k},y_{k},\theta_{k})^{T}$ is the state vector, $\theta_{k}$ is
the heading angle of the vehicle, counter clockwise from the positive
$x-$axis, and $\omega_{k}\in\mathbb{U}=[-\pi,\pi]\,s^{-1}$ is the control
input, which is the rate of change of heading angle $\theta_{k}$. We assume a
constant vehicle speed $V=5\,m/s$. The measurement can be seen as a noisy
position measurement. The random variables $\zeta_{0},w_{k}$ and $v_{k}$
follow the same assumptions for (1), and:
$\mathcal{W}=\mathcal{N}(0,\text{diag}(.2,.2,.1)),\,\mathcal{V}=\mathcal{N}(0,\text{diag}(.1,.1,.1))$
and the particles $\Xi$ representing $p_{0}$, the density of $\zeta_{0}$, are
sampled according to a density
$\mathcal{N}((10,0,-\pi/2)^{T},\text{diag}(.2,.2,.2))$ 555The notation:
$\mathcal{N}(\mu,\Sigma)$ represents a multivariate Gaussian with mean $\mu$
and covariance $\Sigma$, $\text{diag}(e)$ is the square diagonal matrix with
diagonal elements $e$..

This system has the objective to follow a circular orbit while avoiding
multiple obstacles with probability $1-\epsilon$, where $\epsilon=15\%$. A CBF
controller is well-suited to this objective for a deterministic system.
However, the stochastic nature of the dynamics and the partial access to the
states hinder the immediate implementation of a CBF-based control. Instead, we
pick a surrogate deterministic dynamics model

$\displaystyle\dot{p}_{x}=V_{x},\,\dot{p}_{y}=V_{y},$ (11)

where $p_{x}=p_{x}(t)$ and $p_{y}=p_{y}(t)$ are the coordinates of the
vehicle, $V_{x}$ and $V_{y}$ are the velocity inputs.

###  V-A Level 1: baseline control $u_{0}$ for asymptotic behavior

Suppose we use a baseline safety-agnostic controller, $u_{0}$, based on a
vector field navigation approach [15]. The vectors of this field represent the
desired heading angle to follow an orbit. In this example, the path is a
clockwise circular orbit with radius $r=10\,m$ and a center at the origin. The
baseline controller is then derived according to this vector field. The
desired heading angle is defined as

$\displaystyle\theta^{d}=\gamma-\frac{\pi}{2}-\tan^{-1}\left(k(d-r)\right),$
(12)

where $d$ and $\gamma$ are, respectively, the distance and the angular
position of the vehicle with respect to the orbit’s center,
$\gamma=\text{atan2}(p_{y},p_{x})$. The angular position angle is measured
from the positive $x-$axis, and in a counter-clockwise direction. We pick a
gain $k=0.3$. The resulting vector field is visualized in Figure 1.

Figure 1: The vector field defined by a constant magnitude (speed) and an
angle given by (12). The red curve is a circular orbit of $10\,m$ radius.

The baseline control can be defined as
$u_{0}=(V\cos(\theta^{d}),V\sin(\theta^{d}))$, where $V=5\,m/s$ and
$\theta^{d}$ as in (12).

###  V-B Level 2: CBF control $u^{\star}$ for obstacle avoidance

While following the orbit, the vehicle has to avoid three obstacles,
represented as circular objects with centers
$\\{(9,-5),(-10,-9),(-7,10)\\}\,m$ and radii $\\{3,4,3\\}\,m$, respectively.
For the multiple obstacles, we rely on the concept of the Boolean
compositional nonsmooth barrier function [9]. The quadratic program, analogous
to the one in Proposition 3 therein,

$\displaystyle u^{\star}(p_{x},p_{y})$
$\displaystyle=\operatorname*{arg\,min}_{u}\lVert u-u_{0}\rVert^{2},$ (13)
$\displaystyle\hskip-28.45274pt\text{s.t.:}\frac{\partial
h_{m}(p_{x},p_{y})}{\partial(p_{x},p_{y})}^{T}u\geq-\alpha(h_{m}(p_{x},p_{y})),\,m=1,2,3.$

The functions $h_{m}(x,y)$ correspond to each obstacle, and defined as

$h_{m}(p_{x},p_{y})=(p_{x}-p_{x}^{m})^{2}+(p_{y}-p_{y}^{m})^{2}-(r^{m})^{2},$

where $(p_{x}^{m},p_{y}^{m}),r^{m}$ are the coordinates and radius of obstacle
$m$. We pick the class $\mathcal{K}$ function $\alpha(h_{m})=0.05h_{m}$ to
induce a gradual variation in the vector field around these obstacles. The
quadratic program is then solved for a grid over $[-15,15]\times[-15,15]$ in
the state-space. The resulting $u^{\star}=(u^{\star}(1),u^{\star}(2))^{T}$ is
mapped back to a desired heading angle, we denote it
$\theta^{\star}=\text{atan2}(u^{\star}(2),u^{\star}(1))$. The resulting vector
field is shown in Figure 2. This angle, since acquired using (13), is a
function of the vehicle’s position, i.e.
$\theta^{\star}=\theta^{\star}(p_{x},p_{y})$.

Figure 2: The vector field defined by $\theta^{\star}$, induced from the
quadratic program (13). The red curve is a circular orbit of $10\,m$ radius,
and the blue disks are the obstacles to be avoided by the vehicle.

###  V-C Level 3: CIDA control $u^{\\#}$ for the stochastic system

For the state-space system (9),(10), let
$\hat{\zeta}_{k}=(\hat{x}_{k},\hat{y}_{k},\hat{\theta}_{k})^{T}$ denote the
sample average of the particle filtered density at time $k$. The number of
particles in the filter we use is $L=1000$. We define the certainty
equivalence controller (CE), using the error signal between
$\theta^{\star}(\hat{x}_{k},\hat{y}_{k})$ and $\hat{\theta}_{k}$, by
$\omega_{k}^{\text{CE}}=\text{sat}\left(5(\theta^{\star}-\hat{\theta}_{k}),-\pi,\pi\right)$.
This function saturates the first argument above at $\pi$ and below at $-\pi$,
such that $\omega_{k}^{\text{CE}}\in\mathbb{U}=[-\pi,\pi],\,\forall k$. The
result of applying this controller is depicted in Figure 3, which shows the
resulting true simulated trajectory that violates the constraints $54$ times
over $750$ time-steps.

Figure 3: The true simulated vehicle’s position for $750$ time-steps, when the
certainty equivalence control is applied in feedback.

Next, we apply our algorithm CIDA with: $\omega^{\\#}=\pi(\zeta_{0}^{\prime})$
where
$\pi(\zeta_{k}^{\prime})=\text{sat}\left(5(\theta^{\star}(x_{k}^{\prime},y_{k}^{\prime})-\hat{\theta}_{k}^{\prime}),-\pi,\pi\right)$,
$M=R=150,\,N=10,\,\gamma=1$, and statistical violation tolerance $\alpha=5\%$.
According to Theorem 1, these values enforce a probabilistic violation
tolerance $\leq\epsilon=15\%$, and with confidence of at least
$1-\delta=95\%$. To avoid running into infeasibility issues, hard constraints
are replaced by soft ones and the control sequence $\\{u_{k}^{\star}\\}_{k}$
in CIDA with the least $\max_{k}a_{k}$ is chosen.

Figure 4 illustrates a simulated trajectory using $\omega_{k}^{\\#}$ in
feedback. Compared to the certainty equivalence control (Figure 3), CIDA takes
into account the original dynamics (1) and their stochastic nature, and hence,
is more capable of enforcing safety. With CIDA, the true simulated state
violated the constraints for $15$ times compared to $54$ in CE, resulting in a
controller that is $3.5$ times safer than the standard method.666The
associated code: https://github.com/msramada/Control-Importance-Distribution-
Algorithm-CIDA-

Figure 4: The true simulated vehicle’s position for $750$ time-steps, when
CIDA is implemented and the control $\omega^{\\#}$ is applied in feedback.

The simulations were implemented using Python, an interpretted programming
language, on an M1-chip 2021 MacBook Pro with 16.00 GB of RAM. The average
running time for each time-step of CIDA is $\approx 7$ seconds, compared to
0.07 seconds for CE. This can be vastly reduced via optimized parallel
computing.

##  VI Conclusion

This paper presents a method for generating probabilistically safe control for
nonlinear stochastic state uncertain systems. Though generally intractable, we
show that by limiting the search space based on a surrogate deterministic
controller, a safe controller over a prediction horizon can be efficiently
generated and guaranteed by a predefined margin $1-\epsilon$. We demonstrate
the tradeoff between computational speed and safety compared with a standard
certainty equivalence method. Algorithm 1 (CIDA) bears a passing resemblance
to both model reference adaptive control [16], and importance sampling from
Monte Carlo integration literature [8]. This is due to the use of focused
sampling of the control space, guided by a surrogate model that is both:
“close enough” to the original stochastic dynamics and “simple enough” to
admit nonlinear control design such as a CBF. However, these notions of
closeness/simplicity have to be further investigated and, possibly, made
quantifiable. Moreover, the recursive feasibility/safety condition is not
discussed here; this is challenging in general for scenario/sampling-based
methods, even with linearity and convexity assumptions adopted therein [5].
More investigations of this condition is still required to extend the
guarantees offered by these methods.

## References

  * [1] A. D. Ames, S. Coogan, M. Egerstedt, G. Notomista, K. Sreenath, and P. Tabuada, “Control barrier functions: Theory and applications,” in _European control conference_. IEEE, 2019, pp. 3420–3431.
  * [2] K. J. Åström, _Introduction to stochastic control theory_. Courier Corporation, 2012.
  * [3] J. Breeden, K. Garg, and D. Panagou, “Control barrier functions in sampled-data systems,” _IEEE Control Systems Letters_ , vol. 6, 2021.
  * [4] J. Buch, S.-C. Liao, and P. Seiler, “Robust control barrier functions with sector-bounded uncertainties,” _IEEE Control Systems Letters_ , vol. 6, pp. 1994–1999, 2021.
  * [5] M. C. Campi, S. Garatti, and M. Prandini, “Scenario optimization for mpc,” _Handbook of Model Predictive Control_ , pp. 445–463, 2019.
  * [6] Y. Chen, A. Singletary, and A. D. Ames, “Guaranteed obstacle avoidance for multi-robot operations with limited actuation: A control barrier function approach,” _Control Systems Letters_ , vol. 5, 2020.
  * [7] R. K. Cosner, P. Culbertson, A. J. Taylor, and A. D. Ames, “Robust safety under stochastic uncertainty with discrete-time control barrier functions,” _arXiv preprint arXiv:2302.07469_ , 2023.
  * [8] A. Doucet, S. Godsill, and C. Andrieu, “On sequential monte carlo sampling methods for bayesian filtering,” _Statistics and computing_ , vol. 10, no. 3, pp. 197–208, 2000.
  * [9] P. Glotfelter, J. Cortés, and M. Egerstedt, “Nonsmooth barrier functions with applications to multi-robot systems,” _IEEE control systems letters_ , vol. 1, no. 2, pp. 310–315, 2017.
  * [10] W. Hoeffding, “Probability inequalities for sums of bounded random variables,” in _The collected works of Wassily Hoeffding_. Springer, 1994.
  * [11] H. K. Khalil, “Nonlinear systems third edition,” _Patience Hall_ , vol. 115, 2002.
  * [12] P. R. Kumar and P. Varaiya, _Stochastic systems: Estimation, identification, and adaptive control_. SIAM, 2015.
  * [13] J. S. Liu and J. S. Liu, _Monte Carlo strategies in scientific computing_. Springer, 2001, vol. 10.
  * [14] J. Luedtke and S. Ahmed, “A sample approximation approach for optimization with probabilistic constraints,” _SIAM Journal on Optimization_ , vol. 19, no. 2, pp. 674–699, 2008.
  * [15] D. R. Nelson, D. B. Barber, T. W. McLain, and R. W. Beard, “Vector field path following for miniature air vehicles,” _IEEE Transactions on Robotics_ , vol. 23, no. 3, pp. 519–529, 2007.
  * [16] P. Parks, “Liapunov redesign of model reference adaptive control systems,” _Transactions on Automatic Control_ , vol. 11, 1966.
  * [17] M. S. Ramadan, R. R. Bitmead, and K. Huang, “State estimation for control: an approach for output-feedback stochastic mpc,” _arXiv preprint arXiv:2303.00873_ , 2023.
  * [18] T. B. Schön, A. Wills, and B. Ninness, “System identification of nonlinear state-space models,” _Automatica_ , vol. 47, no. 1, 2011.
  * [19] C. Striebel, “Sufficient statistics in the optimum control of stochastic systems,” _Journal of Mathematical Analysis and Applications_ , 1965.
  * [20] C. Wang, M. Bahreinian, and R. Tron, “Chance constraint robust control with control barrier functions,” in _American Control Conference_. IEEE, 2021, pp. 2315–2322.
  * [21] G. Williams, B. Goldfain, P. Drews, K. Saigol, J. M. Rehg, and E. A. Theodorou, “Robust sampling based model predictive control with sparse objective information.” in _Robotics: Science and Systems_ , vol. 14, 2018, p. 2018\.