#  Stein Variational Model Predictive Control

Alexander Lambert1,2 Adam Fishman2,3 Dieter Fox2,3
Byron Boots2,3 Fabio Ramos2,4
1 Georgia Tech 2 NVIDIA 3 University of Washington 4 The University of Sydney
E-mail<EMAIL_ADDRESS>
###### Abstract

Decision making under uncertainty is critical to real-world, autonomous
systems. Model Predictive Control (MPC) methods have demonstrated favorable
performance in practice, but remain limited when dealing with complex
probability distributions. In this paper, we propose a generalization of MPC
that represents a multitude of solutions as posterior distributions. By
casting MPC as a Bayesian inference problem, we employ variational methods for
posterior computation, naturally encoding the complexity and multi-modality of
the decision making problem. We propose a Stein variational gradient descent
method to estimate the posterior directly over control parameters, given a
cost function and observed state trajectories. We show that this framework
leads to successful planning in challenging, non-convex optimal control
problems. The supplementary video can be viewed here:
https://youtu.be/pgBCii1NfA0

> Keywords: Model Predictive Control, Variational Inference

##  1 Introduction

Model predictive control (MPC) is a powerful framework for sequential decision
making in robotics [1, 2]. This success can be largely attributed to its
simplicity and scalability in dealing with stochastic, non-stationary
optimization problems encountered on real systems. MPC has been applied
effectively in different areas, including autonomous driving [3, 1, 2],
humanoid locomotion [4], and dextrous manipulation [5]. However, common
approaches to MPC often fall short in their ability to adequately contend with
complex, multi-modal distributions over possible actions. For instance, such
distributions may arise from non-convexity of constraints, such as obstacles
[6, 7] or from multiple goal locations [8]. Although common sampling-based SOC
algorithms have been shown to exhibit symmetry-breaking in the presence of
sudden disturbances [9] and multiple optima [10], the sampling scheme may
inadequately resolve the true posterior [6]. We require a new class of MPC
algorithms that can effectively contend with complex, non-Gaussian
distributions.

In the following, we formulate MPC as a Bayesian inference problem, where the
target posterior is defined directly over control policy parameters or control
inputs, as opposed to joint probabilities over states and actions [11, 12]. By
taking this perspective, we can construct a relative-entropy minimization
problem to approximate the posterior, and leverage recent advances in
variational inference [13, 14, 15] to derive the optimal distribution over
parameters. Specifically, we use Stein variational gradient descent (SVGD)
[16], to infer a set of solutions which constitute a nonparametric
approximation to the posterior distribution over decision parameters, given
state observations and a defined cost function. The generality of this
approach offers flexibility in designing appropriate MPC algorithms, and is
closely related to common MPC approaches. Additionally, we show how this
framework can be extended to general trajectory optimization problems.

Related Work  The duality between probabilistic inference and optimization for
stochastic optimal control (SOC) has been examined extensively in previous
work [17, 18, 19, 20]. A wide range of approximate inference methods have been
proposed, including Expectation-Propagation (EP) and moment-matching
approaches to control and trajectory optimization [3, 19]. These methods
typically assume a restricted form of the target posterior distribution over
controls (usually in the exponential family), which, under simplifying
assumptions, return the optimal control input. However, these distributions
are often insufficiently expressive for many SOC problems, where non-convexity
may arise from nonlinear dynamics or non-convex cost functions, for instance.
Our interpretation of MPC as an approximate inference problem is perhaps most
closely related to the formulation presented in [18, 11]. Here, an iterative
KL-minimization problem for finite-horizon problems is presented, where the
prior is defined to be the control distribution obtained from the previous
iteration. However, this does not include a strategy for contending with non-
stationary distributions, where the posterior may change between iterations,
and the analysis is restricted to exact inference of the log-partition
function.

Expectation-Maximization (EM) approaches to SOC have also been considered [21,
22, 23], which iteratively optimize a variational lower-bound. However, these
approaches share the limitations inherent with common EM strategies, in that
the form of the posterior distribution is assumed to be known and tractable.
Additionally, representations based on mixture models must contend with mode-
collapse and poor local minima, especially prevalent in higher dimensions [24,
25]. These issues can be mitigated, for example, by introducing entropy
regularization heuristics [22].

The Path Integral (PI) formulation of stochastic optimal control [26, 27, 28]
bears close resemblance to the open-loop-controls characterization of our
proposed approach, where a particular choice of the marginal log-likelihood is
assumed to contain an exponentiated cost. In fact, PI attempts to minimize a
variational lower-bound with respect to the controlled stochastic dynamics.
This follows from the perspective presented here. This comparison applies
equally to KL-control [20], the discrete-time counterpart to PI-control. A
more detailed discussion is included in Appendix E.

The application of SVGD for approximate inference in decision making problems
has been previously explored in the context of maximum entropy reinforcement
learning, where policies are trained using collected experience and updated
offline. In [29], the network-based policy is represented using a set of Stein
particles to generate a single-step action. However, it is unclear how to
appropriately design kernels and evaluate them efficiently in order to counter
the effects of increasing dimensionality (i.e. network size) using this
approach. By contrast, the soft Q-learning algorithm [30] evaluates the SVGD
gradient over single-step actions. This is then subsequently backpropagated
through the policy to update the parameters. To our knowledge, the non-
parametric variational inference approach of [16] has yet to be applied to
MPC, or general SOC and planning problems. The dependence on length of the
planning horizon poses interesting challenges for scaling and sample-efficient
computation. Furthermore, the proposed approach is developed with the online
setting in mind, and addresses the problem of rapidly-changing, non-stationary
target distributions. Lastly, we make explicit connections to existing SOC
methods from a theoretical standpoint.

##  2 Model Predictive Control

We consider the discrete-time stochastic dynamical system:
$\mathbf{x}_{t+1}\sim f\left(\mathbf{x}_{t},\mathbf{u}_{t}\right)$, where at
time $t$, the system state is denoted by $\mathbf{x}_{t}\in\mathbb{R}^{n}$ and
the control input as $\mathbf{u}_{t}\in\mathbb{R}^{d}$. The stochastic
transition map $f:\mathbb{R}^{n}\times\mathbb{R}^{d}\rightarrow\mathbb{R}^{n}$
randomly produces the subsequent state $\mathbf{x}_{t+1}$ and this state is
accompanied by an instantaneous cost $c(\mathbf{x}_{t},\mathbf{u}_{t})$.

Over a time horizon $H$, we define a control trajectory as a sequence of
control inputs beginning at time $t$ :
$U_{t}\triangleq\left(\mathbf{u}_{t},\mathbf{u}_{t+1},...,\mathbf{u}_{t+H-1}\right)$.
Similarly, we define the state trajectory:
$X_{t}\triangleq\left(\mathbf{x}_{t},\mathbf{x}_{t+1},...,\mathbf{x}_{t+H-1},\mathbf{x}_{t+H}\right)$.
The total cost incurred over $H$ timesteps can then be specified as

$\displaystyle
C(X_{t},U_{t})=c_{\mathrm{term}}(\mathbf{x}_{t+H})+\sum_{h=0}^{H-1}c(\mathbf{x}_{t+h},\mathbf{u}_{t+h}),$
(1)

where $c_{\mathrm{term}}(\cdot)$ is the terminal cost. As in [2], we define an
instantaneous feedback policy, $\pi_{\theta_{t}}(\mathbf{x}_{t})$, as a
parameterized probability distribution $p(\mathbf{u}_{t}|\mathbf{x}_{t};\
\theta_{t})$ used to generate a control input at time $t$ given the
$\mathbf{x}_{t}$, i.e., $\mathbf{u}_{t}\sim\pi_{\theta_{t}}(\mathbf{x}_{t})$,
where $\theta_{t}\in\Theta$, is the set of feasible parameter values. MPC
describes the process of finding the optimal, time-indexed sequence of policy
parameters
$\mathrm{\bm{\theta}}_{t}\triangleq\left(\theta_{t},\theta_{t+1},...,\theta_{t+H-1}\right)$,
which determine the sequence of instantaneous feedback policies
$\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}_{t}}\triangleq\left(\pi_{\theta_{t}},\pi_{\theta_{t+1}},...,\pi_{\theta_{t+H-1}}\right)$.
At each time step, we must find $\mathrm{\bm{\theta}}_{t}$, the parameters
that define the optimal policy. We can do this by defining a statistic
$J(\cdot)$ on cost $C(X_{t},U_{t})$ where the minimal $J(\cdot)$ occurs at the
optimal $\mathrm{\bm{\theta}}_{t}$. In real-world situations, the true
dynamics function $f$ is often unavailable, and is commonly estimated using a
parameterized function $\hat{f}_{\xi}$ with parameters $\xi$. As such, we
define the surrogate loss function
$\hat{J}(\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}};\mathbf{x}_{t})=\mathbb{E}_{\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}},\hat{f}_{\xi}}\Big{[}C(X_{t},U_{t})\Big{]}$.
For each MPC-step, the optimal decision is defined as
$\mathrm{\bm{\theta}}_{t}=\operatorname*{arg\,min}_{\mathrm{\bm{\theta}}}\
\hat{J}(\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}};\mathbf{x}_{t})$ which
parameterizes the optimal policy $\pi_{\theta_{t}}$, from which we can sample
a new control value for the first timestep:
$\mathbf{u}_{t}\sim\pi_{\theta_{t}}(\mathbf{x}_{t})=p(\mathbf{u}_{t}|\mathbf{x}_{t};\theta_{t})$.
This is then executed on the physical system to generate the next state value:
$\mathbf{x}_{t+1}\sim f(\mathbf{x}_{t},\mathbf{u}_{t})$.

##  3 Bayesian Model Predictive Control

MPC as Bayesian Inference  Optimal control can be framed as Bayesian inference
by considering the distribution over parameters $\mathrm{\bm{\theta}}$.
Similarly to [18, 12], we introduce an auxiliary binary random variable
$\mathcal{O_{\tau}}=\\{0,1\\}$ to indicate optimality of the state-action
trajectory $\tau=(X_{t},U_{t})$ with respect to the cost function $C(\cdot)$.
Using Bayes’ rule, the distribution of parameters $\mathrm{\bm{\theta}}$
conditioned on the requirement for optimal trajectories
($\mathcal{O_{\tau}}=1$) and the current state $\mathbf{x}_{t}$ can be
expressed as

$\displaystyle
p_{t}(\mathrm{\bm{\theta}}|\mathcal{O_{\tau}}=1;\xi,\mathbf{x}_{t})$
$\displaystyle=\frac{p_{t}(\mathcal{O_{\tau}}=1|\mathrm{\bm{\theta}};\xi,\mathbf{x}_{t})\
p_{t}(\mathrm{\bm{\theta}};\mathbf{x}_{t})}{\int
p_{t}(\mathcal{O_{\tau}}=1|\mathrm{\bm{\theta}};\xi,\mathbf{x}_{t})\
p_{t}(\mathrm{\bm{\theta}};\mathbf{x}_{t})\ d\mathrm{\bm{\theta}}},$ (2)

where explicit dependence on state $\mathbf{x}_{t}$ is included for
generality. In the remaining discussion, we will denote $\mathcal{O_{\tau}}=1$
simply as $\mathcal{O_{\tau}}$ without ambiguity. The likelihood
$p_{t}(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}};\xi,\mathbf{x}_{t})$ is defined
as the marginal probability over all possible control and state trajectories:

$\displaystyle
p_{t}(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}};\xi,\mathbf{x}_{t})$
$\displaystyle=\int\int p(\mathcal{O_{\tau}}|X_{t},U_{t})\ p(X_{t},U_{t}\
|\mathrm{\bm{\theta}};\xi,\mathbf{x}_{t})\ dU_{t}\ dX_{t}$ (3)

where $p(\mathcal{O_{\tau}}|X_{t},U_{t})$ is the probability of optimality
given the observed trajectory $\tau=(X_{t},U_{t})$, and
$p(X_{t},U_{t}|\mathrm{\bm{\theta}};\xi,\mathbf{x}_{t})$ is the joint
probability of state-control trajectories, conditioned on parameters
$\mathrm{\bm{\theta}}$ and assumed dynamics model
$\hat{f}_{\xi}=p_{\xi}(\mathbf{x}_{t+1}|\mathbf{x}_{t},\mathbf{u}_{t})$. In
the discrete-time case, the joint probability can be factorized as

$\displaystyle
p(X_{t},U_{t}|\mathrm{\bm{\theta}};\xi,\mathbf{x}_{t})=\prod_{h=0}^{H-1}p_{\xi}(\mathbf{x}_{t+h+1}|\mathbf{x}_{t+h},\mathbf{u}_{t+h})\pi_{\theta_{h}}(\mathbf{x}_{t+h}),$
(4)

where the current state $\mathbf{x}_{t}$ is observed. If dynamics parameters
must also be inferred, the equations can be extended to include $\xi$ as an
inference variable, where the posterior is defined over both parameters:
$p_{t}(\mathrm{\bm{\theta}},\xi|\mathcal{O_{\tau}};\mathbf{x}_{t})\propto
p_{t}(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}},\xi;\mathbf{x}_{t})\,p_{t}(\mathrm{\bm{\theta}}|\xi;\mathbf{x}_{t})\,p(\xi)$,
suggesting the definition of an alternative latent random variable, such as
$\mathbf{\Theta}=\left(\mathrm{\bm{\theta}},\xi\right)$.

We can further model $p(\mathcal{O_{\tau}}|X_{t},U_{t})$ using a non-negative
function $\mathcal{L}(\tau)\propto p(\mathcal{O_{\tau}}|X_{t},U_{t})$, which
we refer to as the “cost-likelihood". This is defined to be the composition
$\mathcal{L}=g\circ C$ of the cost function $C(\cdot)$ with a monotonically
decreasing function $g(\cdot)$. The likelihood in (3) then takes the form

$\displaystyle
p_{t}(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}};\xi,\mathbf{x}_{t})$
$\displaystyle\propto\int\mathcal{L}(\tau)\ p(\tau\
|\mathrm{\bm{\theta}};\xi,\mathbf{x}_{t})\
d\tau=\mathbb{E}_{\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}},\hat{f}_{\xi}}\Big{[}\
\mathcal{L}(\tau)\Big{]}\ .$ (5)

Nonparametric Bayesian MPC  Instead of performing inference over policy
parameters, the Bayesian MPC formulation can be used for inference over
control input sequences. In this case, the inference variable
$\mathrm{\bm{\theta}}$ is defined as the sequence of open-loop controls:
$\mathrm{\bm{\theta}}\triangleq\left(\mathbf{u}_{t},\mathbf{u}_{t+1},...,\mathbf{u}_{t+H-1}\right)$.
This can be interpreted as a nonparametric version of Bayesian-MPC, as no
assumption is made on the existence of a parametrized policy
$\mathrm{\bm{\pi}}$. Although the relationship in (2) still holds, the
likelihood function
$p_{t}(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}};\xi,\mathbf{x}_{t})$ must then
be re-defined as

$\displaystyle
p_{t}(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}};\xi,\mathbf{x}_{t})=\int
p(\mathcal{O_{\tau}}|X_{t},\mathrm{\bm{\theta}})\
p(X_{t}|\mathrm{\bm{\theta}};\xi,\mathbf{x}_{t})\ dX_{t}\ \propto\
\mathbb{E}_{\hat{f}_{\xi}}\Big{[}\
\mathcal{L}(X_{t},\mathrm{\bm{\theta}})\Big{]}\ ,$ (6)

where $p(X_{t}|\mathrm{\bm{\theta}};\xi,\mathbf{x}_{t})$ is the probability of
state trajectories, conditioned on decisions $\mathrm{\bm{\theta}}$ and
assumed dynamics model
$\hat{f}_{\xi}=p_{\xi}(\mathbf{x}_{t+1}|\mathbf{x}_{t},\mathbf{u}_{t})$. In
the discrete-time case, this can be written as the product of state transition
probabilities along a trajectory:

$\displaystyle
p(X_{t}|\mathrm{\bm{\theta}};\xi,\mathbf{x}_{t})=\prod_{h=0}^{H-1}p_{\xi}(\mathbf{x}_{t+h+1}|\mathbf{x}_{t+h},\theta_{t+h})\
.$ (7)

Furthermore, if we assume a fixed current state
($\mathbf{x}_{t}=\textrm{const.}$), we can apply this model to trajectory
optimization problems by performing approximate inference on the posterior
$p(\mathrm{\bm{\theta}}|\mathcal{O_{\tau}};\xi)$, and taking the maximum a
posteriori estimate,
$\mathrm{\bm{\theta}}^{*}=\operatorname*{arg\,max}_{\mathrm{\bm{\theta}}}\
p(\mathrm{\bm{\theta}}|\mathcal{O_{\tau}};\xi)$.

##  4 Inference for Bayesian MPC

Variational Inference  Variational inference poses posterior estimation as an
optimization task where a candidate distribution $q^{*}(\mathrm{\bm{\theta}})$
within a distribution family $\mathcal{Q}=\\{q(\mathrm{\bm{\theta}})\\}$ is
chosen to best approximate the target distribution
$p(\mathrm{\bm{\theta}}|\mathcal{O_{\tau}})$. This is typically obtained by
minimizing the Kullback-Leibler (KL) divergence:

$\displaystyle q^{*}$
$\displaystyle=\operatorname*{arg\,min}_{q\in\mathcal{Q}}\
D_{KL}\left(q(\mathrm{\bm{\theta}})||p(\mathrm{\bm{\theta}}|\mathcal{O_{\tau}})\right).$
(8)

The solution also maximizes the Evidence Lower Bound (ELBO), as expressed by
the following objective (details in Appendix D):

$\displaystyle q^{*}=\operatorname*{arg\,min}_{q\in\mathcal{Q}}\
-\mathbb{E}_{q}\Big{[}\log
p(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}})\Big{]}+D_{KL}\left(q(\mathrm{\bm{\theta}})\
||\ p(\mathrm{\bm{\theta}})\right).$ (9)

This optimization seeks to maximize the log-likelihood of the observations
with the first term while penalizing for differences between the target and
the prior with the second term. For a high-capacity model space $\mathcal{Q}$
that includes the target distribution, the second term becomes increasingly
small. Selecting a model space $\mathcal{Q}$ with both high capacity and
computational efficiency is critical to variational inference, but remains a
challenging problem.

Stein Variational Gradient Descent  In order to circumvent the challenge of
determining an appropriate $\mathcal{Q}$, while also addressing (8), we
develop an algorithm based on Stein variational gradient descent (SVGD) for
Bayesian inference. The nonparametric nature of SVGD is advantageous as it
removes the need for assumptions on restricted parametric families for $q$.
This approach approximates a posterior $p(\mathrm{\bm{\theta}}|x)$ with a set
of particles $\\{\mathrm{\bm{\theta}}^{i}\\}_{i=1}^{m}$,
$\mathrm{\bm{\theta}}^{i}\in\mathbb{R}^{p}$. The particles are iteratively
updated according to

$\displaystyle\mathrm{\bm{\theta}}^{i}\leftarrow\mathrm{\bm{\theta}}^{i}+\epsilon\bm{\phi}^{*}(\mathrm{\bm{\theta}}^{i})$
(10)

given a step-size $\epsilon$. The function $\bm{\phi}^{*}(\cdot)$ lies in the
unit-ball of an $\mathbb{R}^{p}$-valued reproducing kernel Hilbert space
(RKHS) of the form $\mathcal{H}=\mathcal{H}_{0}\times...\mathcal{H}_{0}$,
where $\mathcal{H}_{0}$ is a scalar-valued RKHS with kernel
$k(\mathrm{\bm{\theta}}^{\prime},\mathrm{\bm{\theta}})$. It characterizes the
optimal perturbation or velocity field (i.e. gradient direction) which
maximally decreases the KL-divergence:

$\displaystyle\bm{\phi}^{*}=\operatorname*{arg\,max}_{\bm{\phi}\in\mathcal{H}}\Big{\\{}-\nabla_{\epsilon}D_{KL}\left(q_{\left[\epsilon\bm{\phi}\right]}||p(\mathrm{\bm{\theta}}|x)\right)\,\mathrm{s.t.}\,\big{|}\big{|}\bm{\phi}\big{|}\big{|}_{\mathcal{H}}\leq
1\Big{\\}},$ (11)

where $q_{\left[\epsilon\bm{\phi}\right]}$ indicates the particle distribution
resulting from taking an update step
$\mathrm{\bm{\theta}}=\mathrm{\bm{\theta}}+\epsilon\bm{\phi}(\mathrm{\bm{\theta}})$.
This has been shown to yield a closed-form solution [16] which can be
interpreted as a functional gradient in RKHS and approximated with the set of
particles:

$\displaystyle\hat{\bm{\phi}}^{*}(\mathrm{\bm{\theta}})=\frac{1}{m}\sum_{j=1}^{m}\Big{[}k(\mathrm{\bm{\theta}}^{j},\mathrm{\bm{\theta}})\nabla_{\mathrm{\bm{\theta}}^{j}}\log
p(\mathrm{\bm{\theta}}^{j}||x)+\nabla_{\mathrm{\bm{\theta}}^{j}}k(\mathrm{\bm{\theta}}^{j},\mathrm{\bm{\theta}})\Big{]}.$
(12)

Eq. 12 has two terms that control different aspects of the algorithm. The
first term is essentially a scaled gradient of the log-likelihood over the
posterior’s particle approximation. The second term is known as the repulsive
force. Intuitively, it pushes particles apart when they get too close to each
other and prevents them from collapsing into a single mode. This allows the
method to approximate complex, possibly multi-modal posteriors in MPC. When
there is only a single particle, the method reduces to a standard optimization
of the log-likelihood or a MAP estimate of the posterior as the repulsive
force term vanishes, i.e.
$\nabla_{\mathrm{\bm{\theta}}}k(\mathrm{\bm{\theta}},\mathrm{\bm{\theta}})=0$.
SVGD’s optimization structure empirically provides better particle efficiency
than other popular sampling procedures, such as Markov Chain Monte Carlo [31].

##  5 Stein Variational MPC

In this section we present our novel method for Stein inference, specifically
designed around MPC requirements. The full algorithm can be found in Appendix
B.

Posterior Sequential Updates  As described in Section 3, the Bayesian
interpretation of MPC seeks to find the posterior distribution over decision
parameters at time $t$. Recalling Eq. 2:

$\displaystyle
p_{t}(\mathrm{\bm{\theta}}|\mathcal{O_{\tau}};\xi,\mathbf{x}_{t})$
$\displaystyle=\frac{p_{t}(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}};\xi,\mathbf{x}_{t})\
\tilde{q}_{t}(\mathrm{\bm{\theta}};\mathbf{x}_{t})}{\int
p_{t}(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}};\xi,\mathbf{x}_{t})\
\tilde{q}_{t}(\mathrm{\bm{\theta}};\mathbf{x}_{t})\
d\mathrm{\bm{\theta}}}\quad.$ (13)

with a prior $\tilde{q}_{t}(\mathrm{\bm{\theta}};\mathbf{x}_{t})$. Our
approach approximates the posterior over decision parameters using a weighted
set of particles $\\{\mathrm{\bm{\theta}}^{i}\\}^{m}_{i=1}$, where the
proposal $q$ is defined as the empirical distribution
$q(\mathrm{\bm{\theta}})=\sum_{i=1}^{m}w^{i}\delta(\mathrm{\bm{\theta}}^{i})$
with weights evaluated according to:

$\displaystyle w^{i}$
$\displaystyle=\frac{p_{t}(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}}^{i};\xi,\mathbf{x}_{t})\
\tilde{q}_{t}(\mathrm{\bm{\theta}};\mathbf{x}_{t})}{\sum_{j=1}^{m}p_{t}(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}}^{j};\xi,\mathbf{x}_{t})\
\tilde{q}_{t}(\mathrm{\bm{\theta}};\mathbf{x}_{t})}$ (14)

such that $\sum_{i=1}^{m}w^{i}=1$. Following the procedure outlined in
(10)-(12), an SVGD update can be computed for the individual particles by
computing the functional gradient

$\displaystyle\hat{\bm{\phi}}^{*}(\mathrm{\bm{\theta}}^{i})=\frac{1}{m}\sum_{j=1}^{m}\Big{[}k(\mathrm{\bm{\theta}}^{j},\mathrm{\bm{\theta}}^{i})\nabla_{\mathrm{\bm{\theta}}^{j}}\log
p_{t}(\mathrm{\bm{\theta}}^{j}|\mathcal{O_{\tau}};\xi,\mathbf{x}_{t})+\nabla_{\mathrm{\bm{\theta}}^{j}}k(\mathrm{\bm{\theta}}^{j},\mathrm{\bm{\theta}}^{i})\Big{]}$
(15)

and performing the gradient step:
$\mathrm{\bm{\theta}}^{i}\leftarrow\mathrm{\bm{\theta}}^{i}+\epsilon\hat{\bm{\phi}}^{*}(\mathrm{\bm{\theta}}^{i})$
. The evaluation of (15) requires computation of the log-posterior gradient,
which can be written as the sum of the gradients of both the log-prior and
log-likelihood:

$\displaystyle\nabla_{\mathrm{\bm{\theta}}^{i}}\log
p_{t}(\mathrm{\bm{\theta}}^{i}|\mathcal{O_{\tau}};\xi,\mathbf{x}_{t})$
$\displaystyle=\nabla_{\mathrm{\bm{\theta}}^{i}}\log
p_{t}(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}}^{i};\xi,\mathbf{x}_{t})+\nabla_{\mathrm{\bm{\theta}}^{i}}\log\tilde{q}_{t}(\mathrm{\bm{\theta}}^{i};\mathbf{x}_{t})$
(16)
$\displaystyle=\nabla_{\mathrm{\bm{\theta}}^{i}}\log\mathbb{E}_{\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}^{i}},\hat{f}_{\xi}}\Big{[}\
\mathcal{L}(\tau)\Big{]}+\nabla_{\mathrm{\bm{\theta}}^{i}}\log\tilde{q}_{t}(\mathrm{\bm{\theta}}^{i};\mathbf{x}_{t})$
(17)

The first RHS term requires that we define the cost-likelihood function
$\mathcal{L}$. The SV-MPC framework allows for different possible definitions
for $\mathcal{L}$. We will consider two in particular:

$\displaystyle\bullet\quad$ $\displaystyle\textrm{Exponentiated Utility
(EU):}\quad\mathcal{L}(\tau)=\exp\left(-\alpha
C(X_{t},U_{t})\right),\quad\textrm{where }\alpha>0$ (18)
$\displaystyle\bullet\quad$ $\displaystyle\textrm{Probability of Low Cost
(PLC):}\quad\mathcal{L}(\tau)=\mathbbm{1}_{C\leq C_{\max}}(C(X_{t},U_{t}))$
(19)

It is generally assumed that the cost function $C(X_{t},U_{t})$ is non-
differentiable with respect to the decision parameter $\mathrm{\bm{\theta}}$,
and that resulting expectations are difficult to evaluate analytically. As
such, the gradients can be estimated via Monte-Carlo sampling, where a set of
$N$ control and state trajectory samples are drawn from the policy and the
modeled dynamics: $\\{\tau^{s}\\}_{s=1}^{N}\sim
p(X_{t},U_{t}|\mathrm{\bm{\theta}};\xi,\mathbf{x}_{t})$, where
$\tau^{s}=(X_{t}^{s},U_{t}^{s})$. This leads to the following approximation:

$\displaystyle\nabla_{\mathrm{\bm{\theta}}^{i}}\log\mathbb{E}_{\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}^{i}},\hat{f}_{\xi}}\Big{[}\
\mathcal{L}(\tau)\Big{]}=\frac{\mathbb{E}_{\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}^{i}},\hat{f}_{\xi}}\Big{[}\mathcal{L}(\tau)\nabla_{\mathrm{\bm{\theta}}^{i}}\log\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}^{i}}\Big{]}}{\mathbb{E}_{\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}^{i}},\hat{f}_{\xi}}\Big{[}\mathcal{L}(\tau)\Big{]}}\approx\frac{\sum_{s=1}^{N}\mathcal{L}(\tau^{s})\nabla_{\mathrm{\bm{\theta}}^{i}}\log\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}^{i}}(U_{t}^{s})}{\sum_{s=1}^{N}\mathcal{L}(\tau^{s})}$
(20)

For a single particle ($m=1$), the full gradient in (15) reduces to :
$\bm{\phi}^{*}(\mathrm{\bm{\theta}})=\nabla_{\mathrm{\bm{\theta}}}\log
p_{t}(\mathrm{\bm{\theta}}|\mathcal{O_{\tau}};\xi,\mathbf{x}_{t})$. SVGD then
produces a local MAP estimate of the posterior distribution over
$\mathrm{\bm{\theta}}$. As a result, the SV-MPC update step exhibits strong
similarity to common MPC algorithms (such as MPPI [1] and CEM [32]) depending
on the chosen likelihood function, exhibiting equivalence under parameter
values and choice of prior. Discussion of these comparisons can be found in
Appendix F.

A significant advantage of the SV-MPC formulation is the robustness to highly-
peaked posterior distributions. If particles are initialized poorly, or if the
target posterior changes significantly between time-steps, many particles may
find themselves in regions of low-probability. Indeed, this may occur
frequently for likelihoods
$p(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}}^{i};\xi,\mathbf{x}_{t})$ with
exponentiated cost (see Appendix F, for example). However, the shared gradient
terms in (15) allow these particles to overcome this degeneracy quickly, while
avoiding collapse due to the repulsive term (the reader may refer to Figure 1.
in [33] for an intuitive illustration of this phenomenon.) As a consequence,
SV-MPC avoids the problem of particle depletion often encountered in
Sequential Monte Carlo methods [34].

Kernels for trajectories High-dimensional inference problems pose significant
challenges for SVGD as the repulsive force given by the derivative of the
kernel with respect to the inputs diminishes as the dimensionality increases
[35]. Inspired by probabilistic graphical models and the conditional
independence assumptions encoded in Markov random fields, we tackle this issue
by devising a kernel that factorizes a high-dimensional input into a sum of
kernels defined over cliques of dimensions. This allows the exploitation of
the Markov structure of the trajectories to address the curse of
dimensionality. For example, assume that the posterior over the parameters
$\mathrm{\bm{\theta}}$ satisfies the conditional independence relations
encoded in a graph ${\cal G}=({\cal V},{\cal E})$, with vertices ${\cal V}$,
and edges ${\cal E}$ such that $p(\mathrm{\bm{\theta}})\propto\prod_{d\in{\cal
E}}\psi_{d}(\theta_{d})\prod_{(dt)\in{\cal
E}}\psi_{dt}(\theta_{d},\theta_{t})$, where $\psi_{d}(\theta_{d})$ and
$\psi_{dt}(\theta_{d},\theta_{t})$ are unary and pairwise potential functions
respectively. We define the kernel over parameters as,

$\displaystyle k(\mathrm{\bm{\theta}},\mathrm{\bm{\theta}}^{\prime})$
$\displaystyle=\sum_{d\in{\cal V}}k(\theta_{d},\theta_{d})+\sum_{(d,t)\in{\cal
E}}k(\theta_{(d,t)},\theta^{\prime}_{(d,t)})$ (21)

The kernel is a sum of positive semi-definite kernels, so the result is a
valid reproducing kernel Hilbert space [36] but less sensitive to the curse of
dimensionality. In this paper we adopt the smooth RBF kernel;
$k(\mathrm{\bm{\theta}},\mathrm{\bm{\theta}}^{\prime})=\exp\left\\{-\|\mathrm{\bm{\theta}}-\mathrm{\bm{\theta}}^{\prime}\|^{2}_{2}/h\right\\}$,
where $h$ is evaluated using the median heuristic on the set of particles:
$h=\mathrm{med}\left(\\{\mathrm{\bm{\theta}}^{i}\\}\right)^{2}/\log m$.

Figure 1: 7-DOF manipulation task. The SV-MPC framework is capable of
reasoning over multi-modal distributions of trajectories in high-dimensional
spaces. Here, the controller iteratively explores the posterior over joint-
velocities by simulating trajectories in parallel (green frame) in order to
guide the system (orange frame). Each particle-generated distribution is shown
by a unique coloring over the generated state trajectories, as seen from a
top-down view of the workspace. The robot arm manages to reach the goal (red),
while avoiding poor local minima.

Action Selection The variational inference procedure results in an
approximation of the posterior distribution over $\mathrm{\bm{\theta}}$.
Following this step, a decision must be made on $\mathrm{\bm{\theta}}_{t}$,
such that a control action can be generated from the resulting policy
$\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}_{t}}$ and executed on the real
system. Here we outline two possible methods for choosing an appropriate
$\mathrm{\bm{\theta}}_{t}$.

We can first consider the relative probabilistic weight of the particles as an
approximation to their posterior probabilities:

$\displaystyle w_{i}$
$\displaystyle=\frac{p(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}}^{i};\xi,\mathbf{x}_{t})\
\tilde{q}_{t-1}(\mathrm{\bm{\theta}}^{i})}{\sum_{j=1}^{m}p(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}}^{j};\xi,\mathbf{x}_{t})\
\tilde{q}_{t-1}(\mathrm{\bm{\theta}}^{j})}$ (22) $\displaystyle\approx
p(\mathrm{\bm{\theta}}^{i}|\mathcal{O_{\tau}};\xi,\mathbf{x}_{t}).$ (23)

One strategy to selecting $\mathrm{\bm{\theta}}_{t}$ is to pick the highest-
weighted particle $\mathrm{\bm{\theta}}_{t}=\mathrm{\bm{\theta}}_{i^{*}}$,
which corresponds to the approximate MAP solution:

$\displaystyle i^{*}$ $\displaystyle=\operatorname*{arg\,max}_{i}[w_{i}]$ (24)
$\displaystyle\approx\operatorname*{arg\,max}_{i}p(\mathrm{\bm{\theta}}^{i}|\mathcal{O_{\tau}};\xi,\mathbf{x}_{t}).$
(25)

An alternative approach is to randomly sample from the posterior distribution,
which can be approximated by sampling from the set of particles according to
their weight $w_{i}$. This can be performed by drawing from the categorical
distribution over particle weights: $i^{*}\sim\mathrm{Cat}(w_{i})$.

Shifting the distribution The prior
$\tilde{q}_{t}(\mathrm{\bm{\theta}};\mathbf{x}_{t})$ is defined as the
marginalized transition over the approximate posterior distribution obtained
from the previous iteration:
$\tilde{q}_{t}(\mathrm{\bm{\theta}};\mathbf{x}_{t})=\int
p_{t}(\mathrm{\bm{\theta}}|\mathrm{\bm{\theta}}_{t-1};\mathbf{x}_{t})q_{t-1}(\mathrm{\bm{\theta}}_{t-1})d\mathrm{\bm{\theta}}_{t-1}$,
with the transition probability
$p_{t}(\mathrm{\bm{\theta}}|\mathrm{\bm{\theta}}_{t-1};\mathbf{x}_{t})$
including a dependence on the currently observed state $\mathbf{x}_{t}$ for
generality. This operation is akin to the prediction step commonly found in
Bayes filtering and sequential Monte-Carlo methods, and can be interpreted as
a probabilistic version of the shift operator defined in [2] which serves to
bootstrap the previous MPC solution to initialize the current iteration.
Further details can be found in Appendix C.

Non-parametric SV-MPC  In the setting where inference must be performed over
open-loop controls $\mathrm{\bm{\theta}}_{t}=U_{t}$, we can define the cost-
likelihood as a function of state: $\mathcal{L}=\mathcal{L}(X_{t})$. A cost on
control can be replaced by defining an additional prior factor
$p(\mathrm{\bm{\theta}})$, which can be combined with the transition
probability above. For further details, refer to Appendix G.

Trajectory optimization The variational inference framework defined by SV-MPC
can be modified to accommodate general motion planning problems common to many
robotics applications. This special case can be considered by (1) using the
non-parametric formulation, (2) assuming a stationary posterior distribution
(no shifting), and (3) defining a deterministic dynamics model $\bar{f}$ such
that state trajectory probabilities can be represented as
$p(X|\mathrm{\bm{\theta}})=\delta(X-\bar{f}(\mathrm{\bm{\theta}},x_{0}))$. A
prior over sequences $p(\mathrm{\bm{\theta}})$ can be defined to encourage
desired behavior such as smoothness [37]. Because individual particles are not
guaranteed to produce a MAP estimate once the SVGD optimization has converged,
they can be subsequently refined by applying stochastic gradient descent (SGD)
using posterior gradients without kernelization. Selection of a feasible and
optimal plan can be generated by simply picking the best particle. The
resulting algorithm, SV-TrajOpt, is described in Algorithm 2, with the
likelihood gradient derived in Appendix H.

##  6 Experiments

In the following section, apply SV-MPC to common robotics problems:
navigation, manipulation and locomotion. We end with an example of the SV-
TrajOpt algorithm applied to a planar motion-planning scenario. All control
algorithms were implemented in PyTorch, with batched gradient computation
across particles and parallel generation of rollouts using either simulated or
analytic dynamical models. Additional experimental details and results can be
found in the Appendix.

Planar Navigation  We construct a 2D robot navigation task, where a holonomic
point-robot must reach a target location while avoiding obstacles (Figure 2).
Hitting an obstacle will cause the agent to "crash" and prevent any further
movement. This added non-differentiability makes the problem particularly
challenging, but also suitable for sampling-based control schemes. The system
exhibits stochastic dynamics, and is defined as a double-integrator model with
additive Gaussian noise. The SV-MPC controller with exponentiated-utility (EU)
is compared against MPPI (Figure 2) and CEM. We provide additional
quantitative results in (Appendix K).

Figure 2: Planar Navigation Task. The robot (orange dot) attempts to reach the
goal location (red cross) while avoiding obstacles. Each frame depicts the
environment state at a particular time-step, along with the distributions of
sampled state-trajectory rollouts generated by the MPC controllers using the
modeled dynamics. Each trajectory color is associated with a single particle
from SV-MPC. The multi-modal distribution of SV-MPC is able to explore
passages between obstacles and find shorter paths to the goal. Figure 3:
Manipulation. Examples of end-effector Cartesian trajectories resulting from
application of different MPC algorithms on the Franka reaching experiment. The
relative distance to the fixed target location is plotted over the length of
each episode. The dashed red line indicates the coordinates of the target. The
sample-averaged terminal cost for the final state
$\overline{C_{T}(\mathbf{x}_{T})}$ is evaluated over the 24 independent
trials. With four particles ($m=4$), SV-MPC with Exponentiated utility
likelihood manages to avoid bad local minima, despite higher-variance
gradients due to fewer samples used to evaluate gradients ($\mathrm{ns}=32$
vs. $\mathrm{ns}=128$).

Manipulation  We demonstrate SV-MPC on a 7-DOF reaching task (Figure 1).
Velocity-based control commands are generated in the configuration space of a
simulated robot manipulator, which must reach a stationary goal in its work-
space. We leverage the GPU-accelerated Isaac-Gym library [38] for parallel
computation of trajectory rollouts in simulation during MPC iterations.

A sampling-based SV-MPC controller is compared against MPPI and CEM for an
open-loop, constant-covariance Gaussian control distribution:
$\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}}=\mathcal{N}(U_{t};\mathrm{\bm{\theta}},\Sigma)$.
The obstacles and the target are placed in order to demonstrate a local-minima
trap: given a finite $H$, the optimal control solution is to move left.
However, the opening between the obstacles in this direction is too narrow for
the robot to move through. To avoid getting stuck, a sampling-based control
scheme must generate a sequence which will move the robot in the other
direction. For a uni-modal distribution, this may have a very low-probability,
and recovery will not occur. Increasing the sampling covariance would mitigate
this, but would require a larger amount of samples to reduce variance.

Examples of trajectory executions are shown in Figure 3. The total number of
generated control samples is held constant across algorithms. Both MPPI and
CEM tend towards the sub-optimal local minimum, leading the robot to get stuck
between two obstacles. Using a particle-based representation of the posterior,
SV-MPC can resolve multiple optima simultaneously, switching to lower-cost
modes around obstacles and successfully reaching the goal.

Stochastic Half-Cheetah To test our approach on a complex nonlinear system
with discontinuous dynamics, we consider an environment common in many
Reinforcement Learning benchmarks: the Half-Cheetah [39]. We use a stochastic
version of the dynamics with additive noise in the control space. The
cumulative rewards over multiple trials are plotted in Figure 4.

(a) Exponentiated Utility (EU)

(b) Probability of Low-Cost (PLC)

Figure 4: Stochastic HalfCheetah. Comparisons of cumulative-reward
distributions over 16 independent trials, with mean and standard deviations
shown. SV-MPC is capable of finding high-reward trajectories, using the same
total amount of samples as MPPI and CEM.

Motion Planning  We use the SV-TrajOpt algorithm to infer a distribution over
optimal control sequences for a planar motion planning problem on a point
robot (Figure 5). The robot must find a velocity-based control sequence which
results in a low-cost, feasible path around a set of obstacles to reach the
goal. The occupancy map is fully-differentiable, allowing a gradient on
obstacle cost to be computed numerically. A ‘smoothness’ prior is defined over
velocities as a multi-variate Gaussian with a tri-diagonal precision matrix,
which favors low-acceleration trajectories.

Figure 5: Motion Planning. SV-TrajOpt is applied to a velocity-controlled
planar robot with fully-differentiable costs. Each blue state-trajectory
results from a single particle control-sequence. Particles are randomly
initialized from the prior ($t=0$), and are optimized until convergence
($t=10,\,100$). Independent local MAP approximations are generated after 50
SGD iterations, with the lowest-cost particle shown in green.

##  7 Conclusion

A novel formulation for Bayesian model predictive control is presented, where
inference is performed directly over the control parameters and inputs. An
algorithm for approximate inference is then proposed, where the posterior is
represented as a set of particles and is optimized via SVGD. In contrast to
pure Monte-Carlo sampling methods, gradient-based information can be exploited
to improve particle efficiency, where many computationally-intensive
operations can be run in parallel using effective GPU implementation. The
flexibility of the approach can accommodate different cost transformations to
modulate risk-seeking behavior, and can naturally be extended to trajectory
optimization problems. We compare against common MPC baselines, demonstrating
improved performance on a variety of control tasks.

## References

  * Williams et al. [2018] G. Williams, P. Drews, B. Goldfain, J. M. Rehg, and E. A. Theodorou.  Information-theoretic model predictive control: Theory and applications to autonomous driving.  _IEEE Transactions on Robotics_ , 34(6):1603–1622, 2018.
  * Wagener et al. [2019] N. Wagener, C.-A. Cheng, J. Sacks, and B. Boots.  An online learning approach to model predictive control.  In _Robotics: Science and Systems_ , 2019.
  * Williams et al. [2016] G. Williams, P. Drews, B. Goldfain, J. M. Rehg, and E. A. Theodorou.  Aggressive driving with model predictive path integral control.  In _2016 IEEE International Conference on Robotics and Automation (ICRA)_ , pages 1433–1440. IEEE, 2016.
  * Kasaei et al. [2019] M. Kasaei, N. Lau, and A. Pereira.  A robust closed-loop biped locomotion planner based on time varying model predictive control.  _arXiv preprint arXiv:1909.06873_ , 2019.
  * Kumar et al. [2016] V. Kumar, E. Todorov, and S. Levine.  Optimal control with learned local models: Application to dexterous manipulation.  In _2016 IEEE International Conference on Robotics and Automation (ICRA)_ , pages 378–383. IEEE, 2016.
  * Kappen [2005] H. J. Kappen.  Path integrals and symmetry breaking for optimal control theory.  _Journal of statistical mechanics: theory and experiment_ , 2005(11):P11011, 2005.
  * Gómez et al. [2016] V. Gómez, S. Thijssen, A. Symington, S. Hailes, and H. J. Kappen.  Real-time stochastic optimal control for multi-agent quadrotor systems.  In _Twenty-Sixth International Conference on Automated Planning and Scheduling_ , 2016.
  * Wiegerinck et al. [2012] W. Wiegerinck, B. v. d. Broek, and H. Kappen.  Stochastic optimal control in continuous space-time multi-agent systems.  _arXiv preprint arXiv:1206.6866_ , 2012.
  * Williams et al. [2018] 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_ , 2018.
  * Kappen [2005] H. J. Kappen.  Linear theory for control of nonlinear stochastic systems.  _Physical review letters_ , 95(20):200201, 2005\.
  * Rawlik et al. [2013] K. Rawlik, M. Toussaint, and S. Vijayakumar.  On stochastic optimal control and reinforcement learning by approximate inference.  In _Twenty-Third International Joint Conference on Artificial Intelligence_ , 2013.
  * Levine [2018] S. Levine.  Reinforcement learning and control as probabilistic inference: Tutorial and review.  _arXiv preprint arXiv:1805.00909_ , 2018.
  * Jordan et al. [1999] M. I. Jordan, Z. Ghahramani, and et al.  An introduction to variational methods for graphical models.  In _Machine Learning_ , pages 183–233. MIT Press, 1999.
  * Beal [2003] M. J. Beal.  Variational algorithms for approximate bayesian inference.  Technical report, University College London, 2003.
  * Wainwright and Jordan [2008] M. Wainwright and M. Jordan.  Graphical Models, Exponential Families, and Variational Inference.  _Foundations and Trends in Machine Learning_ , 1(1-2):1–305, 2008.
  * Liu and Wang [2016] Q. Liu and D. Wang.  Stein variational gradient descent: A general purpose bayesian inference algorithm.  In D. D. Lee, M. Sugiyama, U. V. Luxburg, I. Guyon, and R. Garnett, editors, _Advances in Neural Information Processing Systems 29_ , pages 2378–2386. Curran Associates, Inc., 2016.
  * Todorov [2008] E. Todorov.  General duality between optimal control and estimation.  In _2008 47th IEEE Conference on Decision and Control_ , pages 4286–4292. IEEE, 2008.
  * Rawlik et al. [2010] K. Rawlik, M. Toussaint, and S. Vijayakumar.  Approximate inference and stochastic optimal control.  _arXiv preprint arXiv:1009.3958_ , 2010.
  * Toussaint [2009] M. Toussaint.  Robot trajectory optimization using approximate inference.  In _Proceedings of the 26th annual international conference on machine learning_ , pages 1049–1056. ACM, 2009.
  * Kappen et al. [2012] H. J. Kappen, V. Gómez, and M. Opper.  Optimal control as a graphical model inference problem.  _Machine learning_ , 87(2):159–182, 2012.
  * Toussaint and Storkey [2006] M. Toussaint and A. Storkey.  Probabilistic inference for solving discrete and continuous state markov decision processes.  In _Proceedings of the 23rd international conference on Machine learning_ , pages 945–952, 2006.
  * Okada and Taniguchi [2019] M. Okada and T. Taniguchi.  Variational inference mpc for bayesian model-based reinforcement learning.  In _Conference on Robotics Learning (CoRL)_ , 2019.
  * Watson et al. [2020] J. Watson, H. Abdulsamad, and J. Peters.  Stochastic optimal control as approximate input inference.  In _Conference on Robot Learning_ , pages 697–716, 2020.
  * Bishop [2006] C. M. Bishop.  _Pattern recognition and machine learning_.  springer, 2006.
  * Ormoneit and Tresp [1998] D. Ormoneit and V. Tresp.  Averaging, maximum penalized likelihood and bayesian estimation for improving gaussian mixture probability density estimates.  _IEEE Transactions on Neural Networks_ , 9(4):639–650, 1998.
  * Theodorou et al. [2010] E. Theodorou, J. Buchli, and S. Schaal.  A generalized path integral control approach to reinforcement learning.  _The Journal of Machine Learning Research_ , 11:3137–3181, 2010.
  * Theodorou and Todorov [2012] E. A. Theodorou and E. Todorov.  Relative entropy and free energy dualities: Connections to path integral and kl control.  In _2012 IEEE 51st IEEE Conference on Decision and Control (CDC)_ , pages 1466–1473. IEEE, 2012.
  * Theodorou [2015] E. A. Theodorou.  Nonlinear stochastic control and information theoretic dualities: Connections, interdependencies and thermodynamic interpretations.  _Entropy_ , 17(5):3352–3375, 2015.
  * Liu et al. [2017] Y. Liu, P. Ramachandran, Q. Liu, and J. Peng.  Stein variational policy gradient.  In _Conference on Uncertainty on Artificial Intelligence (UAI)_ , 2017\.
  * Haarnoja et al. [2017] T. Haarnoja, H. Tang, P. Abbeel, and S. Levine.  Reinforcement learning with deep energy-based policies.  In _International Conference on Machine Learning (ICML)_ , pages 1352–1361, 2017.
  * Chen et al. [2019] W. Y. Chen, A. Barp, F.-X. Briol, J. Gorham, M. Girolami, L. Mackey, and C. J. Oates.  Stein point markov chain monte carlo.  In _ICML_ , 2019.
  * Botev et al. [2013] Z. I. Botev, D. P. Kroese, R. Y. Rubinstein, and P. L’Ecuyer.  The cross-entropy method for optimization.  In _Handbook of statistics_ , volume 31, pages 35–59. Elsevier, 2013\.
  * [33] Q. Liu.  Stein variational gradient descent: Theory and applications.
  * Kantas et al. [2009] N. Kantas, J. Maciejowski, and A. Lecchini-Visintini.  Sequential monte carlo for model predictive control.  In _Nonlinear model predictive control_ , pages 263–273. Springer, 2009.
  * Zhuo et al. [2018] J. Zhuo, C. Liu, J. Shi, J. Zhu, N. Chen, and B. Zhang.  Message passing Stein variational gradient descent.  In J. Dy and A. Krause, editors, _Proceedings of the 35th International Conference on Machine Learning_ , volume 80 of _Proceedings of Machine Learning Research_ , pages 6018–6027, Stockholm, Sweden, 10–15 Jul 2018. PMLR.
  * Schölkopf and Smola [2001] B. Schölkopf and A. Smola.  _Learning with Kernels - Support Vector Machines, Regularization, Optimization and Beyond_.  MIT Press, Cambridge, MA, 2001.
  * Mukadam et al. [2018] M. Mukadam, J. Dong, X. Yan, F. Dellaert, and B. Boots.  Continuous-time gaussian process motion planning via probabilistic inference.  _The International Journal of Robotics Research_ , 37(11):1319–1340, 2018.
  * Macklin et al. [2019] M. Macklin, K. Erleben, M. Müller, N. Chentanez, S. Jeschke, and V. Makoviychuk.  Non-smooth newton methods for deformable multi-body dynamics.  _ACM Transactions on Graphics (TOG)_ , 38(5):1–20, 2019.
  * Brockman et al. [2016] G. Brockman, V. Cheung, L. Pettersson, J. Schneider, J. Schulman, J. Tang, and W. Zaremba.  Openai gym.  _arXiv preprint arXiv:1606.01540_ , 2016.
  * Varadhan [1984] S. S. Varadhan.  _Large deviations and applications_.  SIAM, 1984.
  * Hartmann et al. [2017] C. Hartmann, L. Richter, C. Schütte, and W. Zhang.  Variational characterization of free energy: Theory and algorithms.  _Entropy_ , 19(11):626, 2017.

##  Appendix A Notation

The table below summarizes notation used throughout the paper.

Table 1: Notations and definitions Notation | Description
---|---
$\mathbf{x}_{t}$ | System state at time $t$
$X_{t}$ | State trajectory: sequence of length $H+1$, from time $t$
$\mathbf{u}_{t}$ | Control input at time $t$
$U_{t}$ | Control trajectory: sequence of length $H$, from time $t$
$\tau$ | State-control trajectory tuple, $\tau=(X_{t},U_{t})$
$\hat{f}_{\xi}(\cdot,\cdot)$ | Modeled system dynamics, parameterized by $\xi$
$C(\cdot,\cdot)$ | Total cost for horizon $H$
$\pi_{\theta_{t}}(\cdot)$ | Policy at time $t$ parameterized by $\theta_{t}$
$\mathrm{\bm{\theta}}_{t}$ | Sequence of policy parameters or control inputs
$\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}_{t}}(\cdot)$ | Sequence of control policies
$\mathcal{L}(\cdot)$ | Cost-likelihood function
$q(\cdot)$ | Approximate posterior distribution
$\phi(\cdot)$ | Perturbation direction or velocity field that decreases
| the KL divergence between the particle distribution
| and target distribution
$\hat{\phi}(\cdot)$ | Approximated perturbation direction over particles
$k(\cdot,\cdot)$ | Kernel specifying a reproducing kernel Hilbert space

##  Appendix B Algorithm

* Components which apply only in the presence of a parameterized policy $\mathrm{\bm{\pi}}$ are marked in red.

[3pt] Input: Initial state $\mathbf{x}_{0}$, dynamics $\hat{f}_{\xi}$, cost-
likelihood $\mathcal{L}$, prior $p_{0}(\mathrm{\bm{\theta}};\cdot)$, kernel
$k$, policy $\mathrm{\bm{\pi}}$

Initialize
$\tilde{q}_{0}(\mathrm{\bm{\theta}})=p_{0}(\mathrm{\bm{\theta}};\mathbf{x}_{0})$

[3pt] Sample
$\\{\mathrm{\bm{\theta}}^{i}\\}_{i=1}^{m}\sim\tilde{q}_{0}(\mathrm{\bm{\theta}})$

[3pt] for  _$t=0,1,...,T-1$ _ do

for  _$i=1,2,...,m$_ do in parallel

$\nabla_{\mathrm{\bm{\theta}}^{i}}\log
p_{t}(\mathrm{\bm{\theta}}^{i}|\mathcal{O_{\tau}};\xi,\mathbf{x}_{t})=\nabla_{\mathrm{\bm{\theta}}^{i}}\log\mathbb{E}_{{\color[rgb]{1,0.5390625,0.5390625}\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}^{i}},}\hat{f}_{\xi}}\Big{[}\mathcal{L}(\tau)\Big{]}+\nabla_{\mathrm{\bm{\theta}}^{i}}\log\tilde{q}_{t}(\mathrm{\bm{\theta}}^{i})$

end forpar

for  _$i=1,2,...,m$ _ do

$\Delta\mathrm{\bm{\theta}}^{i}\leftarrow\frac{1}{m}\sum_{j=1}^{m}k(\mathrm{\bm{\theta}}^{j},\mathrm{\bm{\theta}}^{i})\nabla_{\mathrm{\bm{\theta}}^{j}}\log
p_{t}(\mathrm{\bm{\theta}}^{j}|\mathcal{O_{\tau}};\xi,\mathbf{x}_{t})+\nabla_{\mathrm{\bm{\theta}}^{j}}k(\mathrm{\bm{\theta}}^{j},\mathrm{\bm{\theta}}^{i})$

[5pt]
$\mathrm{\bm{\theta}}^{i}\leftarrow\mathrm{\bm{\theta}}^{i}+\epsilon\Delta\mathrm{\bm{\theta}}^{i}$

end for

$w^{i}\leftarrow
p(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}}^{i};\mathbf{x}_{t})\tilde{q}_{t}(\mathrm{\bm{\theta}}^{i})$

[5pt] $w^{i}\leftarrow\frac{w^{i}}{\sum_{j=1}^{m}w^{j}}$

[5pt] Pick $\mathrm{\bm{\theta}}^{*}$ (using (22) or (24), for example)

Get control input: $\mathbf{u}_{t}\leftarrow\mathrm{\bm{\theta}}^{*}$ or
sample
$\mathbf{u}_{t}\sim\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}^{*}_{t}}(\mathbf{x}_{t})$

Sample true dynamics: $\quad\mathbf{x}_{t+1}\sim
f(\mathbf{x}_{t},\mathbf{u}_{t})$

Shift particles:
$\quad\tilde{\mathrm{\bm{\theta}}}^{i}=\Phi(\mathrm{\bm{\theta}}^{i})$

Update prior:
$\quad\tilde{q}_{t+1}(\mathrm{\bm{\theta}};\mathbf{x}_{t+1})=\sum_{i=1}^{m}w^{i}p_{t}(\mathrm{\bm{\theta}}|\tilde{\mathrm{\bm{\theta}}}^{i};\mathbf{x}_{t+1})$

end for

Algorithm 1 Stein Variational MPC

Input: Initial state $\mathbf{x}_{0}$, dynamics $f$, cost function $C$, prior
$p_{0}(\mathrm{\bm{\theta}})$, kernel $k$, termination condition
$\mathrm{Done}(\cdot)$

Sample $\\{\mathrm{\bm{\theta}}^{i}\\}_{i=1}^{m}\sim
p_{0}(\mathrm{\bm{\theta}})$

[5pt] Set $\Delta\mathrm{\bm{\theta}}^{i}=\mathrm{Inf}\ \forall\ i\in 1:m$

[5pt] while  _$\mathrm{Done}(\\{\Delta\mathrm{\bm{\theta}}^{i}\\})\textrm{ is
False}$_ do

for  _$i=1,2,...,m$_ do in parallel

Forward dynamics : $X^{i}=X(f,\mathrm{\bm{\theta}}^{i},\mathbf{x}_{0})$

[5pt] $\nabla_{\mathrm{\bm{\theta}}^{i}}\log
p(\mathrm{\bm{\theta}}^{i}|\mathcal{O_{\tau}};\xi)=-\alpha\nabla_{\mathrm{\bm{\theta}}^{i}}C(X^{i})+\nabla_{\mathrm{\bm{\theta}}^{i}}\log
p_{0}(\mathrm{\bm{\theta}}^{i})$

end forpar

for  _$i=1,2,...,m$ _ do

$\Delta\mathrm{\bm{\theta}}^{i}\leftarrow\frac{1}{m}\sum_{j=1}^{m}k(\mathrm{\bm{\theta}}^{j},\mathrm{\bm{\theta}}^{i})\nabla_{\mathrm{\bm{\theta}}^{j}}\log
p(\mathrm{\bm{\theta}}^{j}|\mathcal{O_{\tau}};\xi)+\nabla_{\mathrm{\bm{\theta}}^{j}}k(\mathrm{\bm{\theta}}^{j},\mathrm{\bm{\theta}}^{i})$

[5pt]
$\mathrm{\bm{\theta}}^{i}\leftarrow\mathrm{\bm{\theta}}^{i}+\epsilon\Delta\mathrm{\bm{\theta}}^{i}$

end for

end while

(Optional: SGD refinement)

for  _$iter=1,...,N$ _ do

$\Delta\mathrm{\bm{\theta}}^{i}\leftarrow\nabla_{\mathrm{\bm{\theta}}^{i}}\log
p(\mathrm{\bm{\theta}}^{i}|\mathcal{O_{\tau}};\xi)\quad\forall\ i\in 1:m$

[5pt]
$\mathrm{\bm{\theta}}^{i}\leftarrow\epsilon_{r}\Delta\mathrm{\bm{\theta}}^{i}\quad\forall\
i\in 1:m$

end for

$\mathrm{\bm{\theta}}^{*}=\operatorname*{arg\,max}_{\mathrm{\bm{\theta}}_{i}}\log
p(\mathrm{\bm{\theta}}^{i}|\mathcal{O_{\tau}};\xi)$

[5pt]

Algorithm 2 SV-TrajOpt

##  Appendix C Shift operation

As described in Section 5, the proposal distribution $q$ is propagated after
each round of MPC by marginalizing over the previous solution:

$\displaystyle\tilde{q}_{t+1}(\mathrm{\bm{\theta}};\mathbf{x}_{t+1})=\int
p_{t}(\mathrm{\bm{\theta}}|\mathrm{\bm{\theta}}_{t};\mathbf{x}_{t+1})q_{t}(\mathrm{\bm{\theta}}_{t})d\mathrm{\bm{\theta}}_{t}$
(26)

given a transition probability,
$p_{t}(\mathrm{\bm{\theta}}|\mathrm{\bm{\theta}}_{t-1};\mathbf{x}_{t})$. This
operation serves to approximate the prior at the new iteration. Given the
empirical distribution $q_{t}$, we can simplify the expression above:

$\displaystyle\tilde{q}_{t+1}(\mathrm{\bm{\theta}};\mathbf{x}_{t+1})$
$\displaystyle=\sum_{i=1}^{m}\int
p_{t}(\mathrm{\bm{\theta}}|\mathrm{\bm{\theta}}_{t};\mathbf{x}_{t+1})\
w^{i}\delta_{\mathrm{\bm{\theta}}_{i}}(\mathrm{\bm{\theta}}_{t})d\mathrm{\bm{\theta}}_{t}=\sum_{i=1}^{m}w^{i}p_{t}(\mathrm{\bm{\theta}}|\mathrm{\bm{\theta}}^{i};\mathbf{x}_{t+1})$
(27)

resulting in a mixture of conditional probabilities.

In many open-loop MPC implementations [1, 2], it is assumed that the solution
does not change significantly between each round, given an accurate dynamics
model and sufficiently high controller frequency to resolve the dynamics and
possible perturbations. This motivates a common heuristic used to reduce the
computational burden between timesteps, which is to shift the control
distribution $\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}_{t}}$, forward-in-time
by one step. That is, given an initial parameter sequence
$\mathrm{\bm{\theta}}_{t}=\left(\theta_{t},\theta_{t+1},...,\theta_{t+H-1}\right)$:

$\displaystyle\tilde{\mathrm{\bm{\theta}}}_{t+1}$
$\displaystyle=\Phi(\mathrm{\bm{\theta}}_{t})$ (28)
$\displaystyle=(\theta_{t+1},\theta_{t+2},...,\theta_{t+H-1},\tilde{\theta}_{t+H-1})$
(29)

where the new parameter $\tilde{\theta}_{t+H-1}$ is chosen to reflect the
expected final action. In the implementation, we adopt a similar heuristic,
where the empirical distribution is first shifted deterministically according
to:

$\displaystyle\tilde{\mathrm{\bm{\theta}}}^{i}$
$\displaystyle=\Phi(\mathrm{\bm{\theta}}^{i})\quad\forall\quad i\in 1:m$ (30)

and setting the resulting distribution as
$\tilde{q}_{t}(\mathrm{\bm{\theta}})=q_{t}(\mathrm{\bm{\theta}})$. The shifted
particles, $\tilde{\mathrm{\bm{\theta}}}^{i}$, are then used in the following
iteration to approximate the posterior, and the prior is updated according to
(27). The shift operation can then be summarized by the following two sub-
steps:

  1. 1.

Shift particles:
$\quad\tilde{\mathrm{\bm{\theta}}}^{i}=\Phi(\mathrm{\bm{\theta}}^{i})$

  2. 2.

Update prior:
$\quad\tilde{q}_{t+1}(\mathrm{\bm{\theta}};\mathbf{x}_{t+1})=\sum_{i=1}^{m}w^{i}p_{t}(\mathrm{\bm{\theta}}|\tilde{\mathrm{\bm{\theta}}}^{i};\mathbf{x}_{t+1})$

##  Appendix D Variational Inference

In variational inference, a target distribution
$p(\mathrm{\bm{\theta}}|\mathcal{O_{\tau}})$ is approximated by a candidate
$q^{*}(\mathrm{\bm{\theta}})$, belonging to a specified family of
distributions $\mathcal{Q}=\\{q(\mathrm{\bm{\theta}})\\}$, by minimizing the
KL-divergence:

$\displaystyle q^{*}$
$\displaystyle=\operatorname*{arg\,min}_{q\in\mathcal{Q}}\
D_{KL}\left(q(\mathrm{\bm{\theta}})||p(\mathrm{\bm{\theta}}|\mathcal{O_{\tau}})\right)$
(31)

The optimal distribution $q^{*}$ is also the solution to the following
objective:

$\displaystyle q^{*}=\operatorname*{arg\,max}_{q\in\mathcal{Q}}\
\mathbb{E}_{q}\Big{[}\log
p(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}})\Big{]}-D_{KL}\left(q(\mathrm{\bm{\theta}})\
||\ p(\mathrm{\bm{\theta}})\right)$ (32)

Proof 1:

$\displaystyle q^{*}$
$\displaystyle=\operatorname*{arg\,min}_{q\in\mathcal{Q}}\
D_{KL}\left(q(\mathrm{\bm{\theta}})||p(\mathrm{\bm{\theta}}|\mathcal{O_{\tau}})\right)$
(33) $\displaystyle=\operatorname*{arg\,min}_{q\in\mathcal{Q}}\int\log
q(\mathrm{\bm{\theta}})dq(\mathrm{\bm{\theta}})-\int\log
p(\mathrm{\bm{\theta}}|\mathcal{O_{\tau}})dq(\mathrm{\bm{\theta}})$ (34)
$\displaystyle=\operatorname*{arg\,min}_{q\in\mathcal{Q}}\int\log
q(\mathrm{\bm{\theta}})dq(\mathrm{\bm{\theta}})-\int\log
p(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}})dq(\mathrm{\bm{\theta}})-\int\log
p(\mathrm{\bm{\theta}})dq(\mathrm{\bm{\theta}})+\log Z$ (35)
$\displaystyle=\operatorname*{arg\,min}_{q\in\mathcal{Q}}\int\log
q(\mathrm{\bm{\theta}})dq(\mathrm{\bm{\theta}})-\int\log
p(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}})dq(\mathrm{\bm{\theta}})-\int\log
p(\mathrm{\bm{\theta}})dq(\mathrm{\bm{\theta}})$ (36)
$\displaystyle=\operatorname*{arg\,min}_{q\in\mathcal{Q}}\
\mathbb{E}_{q}\Big{[}\log
q(\mathrm{\bm{\theta}})\Big{]}-\mathbb{E}_{q}\Big{[}\log
p(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}})+\log
p(\mathrm{\bm{\theta}})\Big{]}$ (37)
$\displaystyle=\operatorname*{arg\,max}_{q\in\mathcal{Q}}\
\mathbb{E}_{q}\Big{[}\log
p(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}})\Big{]}-D_{KL}\left(q(\mathrm{\bm{\theta}})\
||\ p(\mathrm{\bm{\theta}})\right)$ (38)

where $Z$ is the normalizing constant. The functional in the objective is also
known as the variational free energy
$F_{q}(\mathrm{\bm{\theta}}):=\mathbb{E}_{q}\Big{[}\log
p(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}})\Big{]}-D_{KL}\left(q(\mathrm{\bm{\theta}})\
||\ p(\mathrm{\bm{\theta}})\right)$.

An alternative derivation, which is relevant to the discussion in Appendix E,
can be found by deriving the lower-bound to the log marginal-likelihood:

Proof 2:

$\displaystyle\log\mathbb{E}_{p}\Big{[}p(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}})\Big{]}$
$\displaystyle=\log\int
p(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}})p(\mathrm{\bm{\theta}})d\mathrm{\bm{\theta}}$
(39) $\displaystyle=\log\int
p(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}})p(\mathrm{\bm{\theta}})d\mathrm{\bm{\theta}}$
(40) $\displaystyle=\log\int
p(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}})\frac{p(\mathrm{\bm{\theta}})}{q(\mathrm{\bm{\theta}})}q(\mathrm{\bm{\theta}})d\mathrm{\bm{\theta}}$
(41)
$\displaystyle=\log\mathbb{E}_{q}\Big{[}p(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}})\frac{p(\mathrm{\bm{\theta}})}{q(\mathrm{\bm{\theta}})}\Big{]}$
(42)
$\displaystyle\geq\mathbb{E}_{q}\Big{[}\log\frac{p(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}})p(\mathrm{\bm{\theta}})}{q(\mathrm{\bm{\theta}})}\Big{]}$
(43) $\displaystyle=\mathbb{E}_{q}\Big{[}\log
p(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}})\Big{]}-\mathbb{E}_{q}\Big{[}\log\frac{p(\mathrm{\bm{\theta}})}{q(\mathrm{\bm{\theta}})}\Big{]}$
(44) $\displaystyle=\mathbb{E}_{q}\Big{[}\log
p(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}})\Big{]}-D_{KL}\left(q(\mathrm{\bm{\theta}})\
||\ p(\mathrm{\bm{\theta}})\right)$ (45)

where (43) results from log-concavity and application of Jensen’s inequality.
Equality is obtained when $q$ matches the posterior probability. In other
words, provided that
$p(\mathrm{\bm{\theta}}|\mathcal{O_{\tau}})\in\mathcal{Q}$, we have:

$\displaystyle
q^{*}(\mathrm{\bm{\theta}})=\frac{p(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}})\
p(\mathrm{\bm{\theta}})}{\int p(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}})\
p(\mathrm{\bm{\theta}})d\mathrm{\bm{\theta}}}=p(\mathrm{\bm{\theta}}|\mathcal{O_{\tau}})$
(46)

As such, we can define the minimization:

$\displaystyle-\log\mathbb{E}_{p}\Big{[}p(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}})\Big{]}$
$\displaystyle=\min_{q\in\mathcal{Q}}-\mathbb{E}_{q}\Big{[}\log
p(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}})\Big{]}+D_{KL}\left(q(\mathrm{\bm{\theta}})\
||\ p(\mathrm{\bm{\theta}})\right)$ (47)

##  Appendix E Connection to Path Integral Control

The equation derived in (47) is well-known in statistical thermodynamics,
where the random variable under consideration is the energy
$C(x):\Omega_{x}\mapsto\mathbb{R}$, a non-negative real-valued measurable
property, and $x\in\mathbb{R}^{n}$ is the state of the system. The Helmholtz
free energy of $C$, with respect to probability density $p$, is defined as the
function :

$\displaystyle-\frac{1}{\alpha}\log\mathbb{E}_{p}\Big{[}\exp\left(-\alpha
C(x)\right)\Big{]},$ (48)

where $\alpha>0$ is the (inverse) temperature. The corresponding variational
inequality, known as the Donsker-Varadahn principle [40, 41], relates the free
energy as the Legendre transform of the entropy:

$\displaystyle-\frac{1}{\alpha}\log\mathbb{E}_{p}\Big{[}\exp\left(-\alpha
C(x)\right)\Big{]}$ $\displaystyle=\min_{q}\
-\frac{1}{\alpha}\mathbb{E}_{q}\Big{[}\log\exp\left(-\alpha
C(x)\right)\Big{]}+\frac{1}{\alpha}D_{KL}\left(q(x)\ ||\ p(x)\right)$ (49)
$\displaystyle=\min_{q}\
\mathbb{E}_{q}\Big{[}C(x)\Big{]}+\frac{1}{\alpha}D_{KL}\left(q(x)\ ||\
p(x)\right)$ (50)

Following the same derivation presented in (39)-(47), the solution is then
found to be the Gibbs distribution:

$\displaystyle q^{*}=\frac{\exp\left(-\alpha
C(x)\right)p(x)}{\int\exp\left(-\alpha C(x)\right)p(x)dx}.$ (51)

In the context of stochastic dynamics, the above is also applicable to random
paths generated by a Markov diffusion process [41]. This can then be extended
to optimal control, by addressing the KL-minimization problem between
controlled and uncontrolled stochastic systems. The connection was developed
and explored for continuous-time dynamics in previous work, such as [27, 28].
Here, the nonlinear-affine dynamics under consideration are subject to
Brownian motion:

$\displaystyle\mathbf{x}_{t+1}$
$\displaystyle=\bar{f}(\mathbf{x}_{t})+G(\mathbf{x}_{t})\mathbf{u}_{t}+B(\mathbf{x}_{t})\mathbf{w}_{t},\quad\mathbf{w}_{t}\sim\mathcal{N}(0,\Sigma)$
(52) $\displaystyle=f(\mathbf{x}_{t},\mathbf{u}_{t})$ (53)

for the discrete-time case. For a SOC expected-cost objective:

$\displaystyle\min_{U_{t}}J(X_{t})=\min_{U_{t}=(\mathbf{u}_{0},...,\mathbf{u}_{T})}\mathbb{E}_{f}\Big{[}c_{\mathrm{term}}(\mathbf{x}_{T})+\sum^{T}_{t=0}c_{t}(\mathbf{x}_{t})+\frac{1}{2}\mathbf{u}_{t}^{\top}R\mathbf{u}_{t}\Big{]}\
,$ (54)

the optimal control distribution could be derived by a change of measure using
Girsanov’s theorem, which was shown to satisfy the Hamilton-Jacobi-Bellman
(HJB) equations for optimality. To solve the HJB equations, an exponentiated
form of the value function, the desirability-function, was defined:
$\Psi(\mathbf{x}_{t})=\exp\left(-\alpha\mathrm{V}(\mathbf{x}_{t})\right)$. By
application of the Feynman-Kac lemma, the transformed HJB partial differential
equation can be solved as:

$\displaystyle\Psi(\mathbf{x}_{t})$ $\displaystyle=\int\exp\left(-\alpha
C(X_{t})\right)p_{f}(X_{t};\mathbf{x}_{t})dX_{t}$ (55)
$\displaystyle=\mathbb{E}_{p_{f}}\Big{[}\exp\left(-\alpha
C(X_{t})\right)\Big{]}$ (56)

where $p_{f}$ denotes the passive dynamics i.e. the probability density of
trajectories $X_{t}$, resulting from the stochastic dynamics
$f(\mathbf{x}_{t},\mathbf{u}_{t})$ with $\mathbf{u}_{t}=0$. Under the
assumption that $R=\frac{1}{\alpha}\Sigma^{-1}$, this can then be used to
evaluate the optimal control law:

$\displaystyle\mathbf{u}^{*}_{t}$
$\displaystyle=-\alpha\Sigma\,G(\mathbf{x}_{t})^{\top}\nabla_{\mathbf{x}}\mathbf{V}(\mathbf{x}_{t})$
$\displaystyle=\Sigma\,G(\mathbf{x}_{t})^{\top}\nabla_{\mathbf{x}}\log\Psi(\mathbf{x}_{t})$
$\displaystyle=\Sigma\,G(\mathbf{x}_{t})^{\top}\frac{\nabla_{\mathbf{x}}\Psi}{\Psi}\
.$ (57)

For receding-horizon control, we can borrow from [1] and consider a discrete-
time case of Path-Integral control for a nonlinear stochastic dynamical
system:

$\displaystyle\mathbf{x}_{t+1}=\bar{f}(\mathbf{x}_{t},\mathbf{v}_{t}),\
\mathbf{v}_{t}\sim p(\mathbf{v}_{t}|\mathbf{u}_{t})$ (58)

with nominal deterministic dynamics $\bar{f}$, commanded control input
$\mathbf{u}_{t}$, and stochastic perturbations $\mathbf{v}_{t}$ which are
exhibited in the control input channel. The uncontrolled system is then
realized when $\mathbf{u}_{t}=0$, and is controlled otherwise. Given a
sequence of perturbations:
$V_{t}=\left(\mathbf{v}_{t},\mathbf{v}_{t+1},...\mathbf{v}_{t+H-1}\right)$, we
can write the resulting state trajectory as $X_{t}=F(\mathbf{x}_{t},V_{t})$,
where $F$ performs consecutive application of the dynamics $\bar{f}$ given
$\mathbf{x}_{t}$ and a sequence $V_{t}$. We can then consider probability
distributions directly over $V_{t}$, with $p(V_{t})$ for uncontrolled dynamics
and $q(V_{t}|U_{t})$ for the controlled system, with the sequence of control
inputs given by
$U_{t}=\left(\mathbf{u}_{t},\mathbf{u}_{t+1},...\mathbf{u}_{t+H-1}\right)$. A
cost on state $C(X_{t})$ is mapped to the random variable $V_{t}$ by the
convolution $S=C\circ F$. Similarly to equations (47) and (50) above, we then
seek a solution to minimize the variational objective:

$\displaystyle q^{*}$ $\displaystyle=\operatorname*{arg\,min}_{q}\
D_{KL}\left(q\,||\,q^{*}(V_{t})\,\right)$ (59)
$\displaystyle=\operatorname*{arg\,min}_{q}\
-\frac{1}{\alpha}\mathbb{E}_{q}\Big{[}\log\exp\left(-\alpha
S(V_{t})\right)\Big{]}+\frac{1}{\alpha}D_{KL}\left(q\ ||\ p\right)$ (60)
$\displaystyle=\operatorname*{arg\,min}_{q}\
\mathbb{E}_{q}\Big{[}S(V_{t})\Big{]}+\frac{1}{\alpha}D_{KL}\left(q\ ||\
p\right)$ (61)

where the optimal distribution is then known to be

$\displaystyle q^{*}(V_{t})$ $\displaystyle=\frac{\exp\left(-\alpha
S(V_{t})\right)p(V_{t})}{\int\exp\left(-\alpha S(V_{t})\right)p(V_{t})dV_{t}}\
.$ (62)

Although we have the form of the optimal control distribution, an analytic
solution is generally intractable due to the partition function. The algorithm
for Model Predictive Path Integral Control (MPPI) [3, 1] proceeds by defining
a surrogate cross-entropy minimization problem:

$\displaystyle\min_{q\in\mathcal{Q}}\ D_{KL}\left(q^{*}(V_{t})\,||\,q\right)$
(63)

for a tractable family of distributions $\mathcal{Q}$, typically fixed-
covariance Gaussians with mean parameters $\mathbf{\mu}_{t}=U_{t}$ and an
equivalent assumption on the controlled dynamics distribution.

In the SV-MPC framework, the original variational objective of Path Integral
control can be addressed, where

$\displaystyle\min_{q\in\mathcal{Q}}\
D_{KL}\left(q\,||\,q^{*}(V_{t})\,\right).$ (64)

We can apply the non-parametric Bayesian-MPC formulation (Setion 3) to solve
for $q^{*}(V_{t})$ directly over $V_{t}$, setting the inference parameter to
be $\mathrm{\bm{\theta}}=V_{t}$ with a prior given by the passive dynamics
$p(\mathrm{\bm{\theta}};\mathbf{x}_{t})=p(V_{t})$. The likelihood term
$p(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}};\xi,\mathbf{x}_{t})$ can be derived
by using the exponentiated-utility $\mathcal{L}(X_{t})=\exp\left(-\alpha
C(X_{t})\right)$ and defining a dirac measure over state trajectories to
denote the probability density of $X_{t}$ given $\mathrm{\bm{\theta}}$:

$\displaystyle p(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}};\xi,\mathbf{x}_{t})$
$\displaystyle\propto\int\exp\left(-\alpha
C(X_{t})\right)\delta(X_{t}-F(\mathbf{x}_{t},\mathrm{\bm{\theta}}))dX_{t}$
(65) $\displaystyle=\exp\left(-\alpha
C(F(\mathbf{x}_{t},\mathrm{\bm{\theta}}))\right)$ (66)
$\displaystyle=\exp\left(-\alpha S(\mathrm{\bm{\theta}})\right)\ .$ (67)

where we drop $\xi$-notation for the dynamics parameters for simplicity.

We then recover the Bayesian formulation:

$\displaystyle q^{*}(\mathrm{\bm{\theta}})$
$\displaystyle=\frac{p(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}};\xi,\mathbf{x}_{t})\
p(\mathrm{\bm{\theta}};\mathbf{x}_{t})}{\int
p(\mathcal{O_{\tau}}|\mathrm{\bm{\theta}};\xi,\mathbf{x}_{t})\
p(\mathrm{\bm{\theta}};\mathbf{x}_{t})d\mathrm{\bm{\theta}}}$ (68)
$\displaystyle=\frac{\exp\left(-\alpha S(\mathrm{\bm{\theta}})\right)\
p(\mathrm{\bm{\theta}};\mathbf{x}_{t})}{\int\exp\left(-\alpha
S(\mathrm{\bm{\theta}})\right)\
p(\mathrm{\bm{\theta}};\mathbf{x}_{t})d\mathrm{\bm{\theta}}}$ (69)
$\displaystyle=p(\mathrm{\bm{\theta}}|\mathcal{O_{\tau}};\xi,\mathbf{x}_{t})$
(70)

Approximate inference on the posterior
$p(\mathrm{\bm{\theta}}|\mathcal{O_{\tau}};\xi,\mathbf{x}_{t})$ can then be
performed using Algorithm 1.

We can further consider the special case of control-affine dynamics of the
form:

$\mathbf{x}_{t+1}=\bar{f}(\mathbf{x}_{t})+G(\mathbf{x}_{t})(\mathbf{u}_{t}+\mathbf{v}_{t}),\quad\mathbf{v}_{t}\sim\mathcal{N}(0,\Sigma).\\\
$ (71)

with passive dynamics:

$\mathbf{x}_{t+1}=\bar{f}(\mathbf{x}_{t})+G(\mathbf{x}_{t})\mathbf{v}_{t},\quad\mathbf{u}_{t}=0\
\forall\ t\ .$ (72)

We refer to this in trajectory-wise form as $X_{t}=F(\mathbf{x}_{t},V_{t})$.

By application of (E), the optimal control action can then be determined :

$\displaystyle\mathbf{u}^{*}_{t}=\Sigma\,G(\mathbf{x}_{t})^{\top}\frac{\nabla_{\mathbf{x}}\Psi}{\Psi}$
$\displaystyle=\Sigma\,G(\mathbf{x}_{t})^{\top}\frac{\nabla_{\mathbf{x}}\mathbb{E}_{V_{t}\sim
p(V_{t})}\Big{[}\exp(-\alpha S(V_{t}))\Big{]}}{\mathbb{E}_{V_{t}\sim
p(V_{t})}\Big{[}\exp(-\alpha S(V_{t}))\Big{]}}$ (73)
$\displaystyle=-\alpha\Sigma\,G(\mathbf{x}_{t})^{\top}\frac{\mathbb{E}_{V_{t}\sim
p(V_{t})}\Big{[}\exp(-\alpha
S(V_{t}))\nabla_{\mathbf{x}}S(V_{t})|_{\mathbf{x}=\mathbf{x}_{t}}\Big{]}}{\mathbb{E}_{V_{t}\sim
p(V_{t})}\Big{[}\exp(-\alpha S(V_{t}))\Big{]}}$ (74)
$\displaystyle\approx-\alpha\Sigma\,G(\mathbf{x}_{t})^{\top}\frac{\sum^{m}_{i=1}\exp(-\alpha
S(\mathrm{\bm{\theta}}_{i}))\nabla_{\mathbf{x}}S(\mathrm{\bm{\theta}}_{i})|_{\mathbf{x}=\mathbf{x}_{t}}}{\sum^{m}_{i=1}\exp(-\alpha
S(\mathrm{\bm{\theta}}_{i}))}$ (75)

where
$\nabla_{\mathbf{x}}S(\mathrm{\bm{\theta}}_{i})|_{\mathbf{x}=\mathbf{x}_{t}}$
is the gradient of the path cost with respect to the current state
$\mathbf{x}_{t}$. This can be expressed in terms of the nominal dynamics by
using the chain rule:

$\nabla_{\mathbf{x}}S(\mathrm{\bm{\theta}}_{i})^{\top}|_{\mathbf{x}=\mathbf{x}_{t}}=\frac{dC(X_{t})}{dX_{t}}\bigg{|}_{X_{t}=F(\mathbf{x}_{t},\mathrm{\bm{\theta}}_{i})}\cdot\frac{dF(\mathbf{x},\mathrm{\bm{\theta}}_{i})}{d\mathbf{x}}\bigg{|}_{\mathbf{x}=\mathbf{x}_{t}}$
(76)

Numerical evaluation of this gradient can be performed via backpropagation for
each $i$-th particle.

##  Appendix F Likelihood functions

Exponentiated Utility (EU)  By picking the cost-likelihood to be:
$\mathcal{L}(\tau)=\exp\left(-\alpha C(X_{t},U_{t})\right)$, we obtain the
likelihood function

$\displaystyle\mathbb{E}_{\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}},\hat{f}_{\xi}}\Big{[}\
\mathcal{L}(\tau)\Big{]}=\mathbb{E}_{\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}},\hat{f}_{\xi}}\Big{[}\exp\left(-\alpha
C(X_{t},U_{t})\right)\Big{]}.$ (77)

This is otherwise known as the Free-Energy of the cost function
$C(\mathbf{x}_{t},\mathbf{u}_{t})$ [27], as well as the "soft maximum" or
"risk-aware" loss. This yields the likelihood-gradient:

$\displaystyle\nabla_{\mathrm{\bm{\theta}}}\log\mathbb{E}_{\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}},\hat{f}_{\xi}}\Big{[}\
\mathcal{L}(\tau)\Big{]}$
$\displaystyle=\nabla_{\mathrm{\bm{\theta}}}\log\mathbb{E}_{\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}},\hat{f}_{\xi}}\Big{[}\exp\left(-\alpha
C(X_{t},U_{t})\right)\Big{]}$ (78)
$\displaystyle=\frac{\mathbb{E}_{\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}},\hat{f}_{\xi}}\Big{[}\exp\left(-\alpha
C(X_{t},U_{t})\right)\nabla_{\mathrm{\bm{\theta}}}\log\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}}\Big{]}}{\mathbb{E}_{\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}},\hat{f}_{\xi}}\Big{[}\exp\left(-\alpha
C(X_{t},U_{t})\right)\Big{]}}$ (79)

As $\alpha\rightarrow\infty$, regions of high-cost are assigned lower
probability, making the distribution of resulting policies risk-averse.
Conversely, as $\alpha\rightarrow 0$, high-cost regions have higher
likelihood, making the policy distributions more risk-seeking.

With this form of the gradient, we can choose a control policy as a sequence
of Gaussian distributions over open-loop controls with fixed covariance: given
that
$\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}_{t}}=\left(\pi_{\theta_{t}},\pi_{\theta_{t+1}},...,\pi_{\theta_{t+H-1}}\right)$,
we have the instantaneous policy
$\pi_{\theta_{t}}=\mathcal{N}(\mu_{t},\Sigma_{t})$, our decision parameter is
thus
$\mathrm{\bm{\theta}}_{t}=\bm{\mu}_{t}=\left(\mu_{t},\mu_{t+1},...\mu_{t+H-1}\right)$,
where $\mu_{t}\in\mathbb{R}^{d}$, $\Sigma_{t}\in\mathbb{R}^{d\times d}$.
Considering the SV-MPC step in (15) for a single particle, we can derive the
update for a parameter element $h\in(t,t+1,...,t+H-1)$:

$\displaystyle\theta^{\prime}_{h}$
$\displaystyle=\theta_{h}+\epsilon\Big{(}\nabla_{\theta_{h}}\log
p_{t}(\mathcal{O_{\tau}}\,|\,\mathrm{\bm{\theta}};\xi,\mathbf{x}_{t})+\nabla_{\theta_{h}}\log\tilde{q}_{t}(\mathrm{\bm{\theta}})\Big{)}$
(80)
$\displaystyle=\theta_{h}+\epsilon\,\frac{\mathbb{E}_{\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}},\hat{f}_{\xi}}\Big{[}\exp\left(-\alpha
C(X_{t},U_{t})\right)\nabla_{\theta_{h}}\log\pi_{\theta_{h}}\Big{]}}{\mathbb{E}_{\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}},\hat{f}_{\xi}}\Big{[}\exp\left(-\alpha
C(X_{t},U_{t})\right)\Big{]}}+\epsilon\nabla_{\theta_{h}}\log\tilde{q}_{t}(\mathrm{\bm{\theta}})$
(81)
$\displaystyle=\theta_{h}+\epsilon\,\Sigma^{-1}_{t}\frac{\mathbb{E}_{\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}},\hat{f}_{\xi}}\Big{[}\exp\left(-\alpha
C(X_{t},U_{t})\right)(\mathbf{u}_{h}-\theta_{h})\Big{]}}{\mathbb{E}_{\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}},\hat{f}_{\xi}}\Big{[}\exp\left(-\alpha
C(X_{t},U_{t})\right)\Big{]}}+\epsilon\nabla_{\theta_{h}}\log\tilde{q}_{t}(\mathrm{\bm{\theta}})$
(82)
$\displaystyle=(I-\epsilon\,\Sigma^{-1}_{t})\,\theta_{h}+\epsilon\,\Sigma^{-1}_{t}\frac{\mathbb{E}_{\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}},\hat{f}_{\xi}}\Big{[}\exp\left(-\alpha
C(X_{t},U_{t})\right)\mathbf{u}_{h}\Big{]}}{\mathbb{E}_{\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}},\hat{f}_{\xi}}\Big{[}\exp\left(-\alpha
C(X_{t},U_{t})\right)\Big{]}}+\epsilon\nabla_{\theta_{h}}\log\tilde{q}_{t}(\mathrm{\bm{\theta}})$
(83)

where the gradient of the log-prior will depend on the particular choice of
transition probability,
$p_{t}(\mathrm{\bm{\theta}}|\tilde{\mathrm{\bm{\theta}}};\mathbf{x}_{t+1})$,
used in the shift operation (see Appendix C). The update reduces to that found
in MPPI [1] if we consider the control distribution to be uncorrelated across
dimensions (as is often done in practice): $\Sigma_{t}=\sigma_{t}^{2}I$.
Setting the step-size to $\epsilon=\sigma^{2}$, and assuming a uniform prior
on controls,
$p_{t}(\theta|\tilde{\theta};\mathbf{x}_{t+1})=\mathcal{U}(\theta_{min},\theta_{max})$:

$\displaystyle\theta^{\prime}_{h}$
$\displaystyle=(I-\sigma_{t}^{2}\,\sigma^{-2}_{t}I)\,\theta_{h}+\sigma_{t}^{2}\,\sigma^{-2}_{t}I\frac{\mathbb{E}_{\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}},\hat{f}_{\xi}}\Big{[}\exp\left(-\alpha
C(X_{t},U_{t})\right)\mathbf{u}_{h}\Big{]}}{\mathbb{E}_{\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}},\hat{f}_{\xi}}\Big{[}\exp\left(-\alpha
C(X_{t},U_{t})\right)\Big{]}}+0$ (84)
$\displaystyle=\frac{\mathbb{E}_{\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}},\hat{f}_{\xi}}\Big{[}\exp\left(-\alpha
C(X_{t},U_{t})\right)\mathbf{u}_{h}\Big{]}}{\mathbb{E}_{\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}},\hat{f}_{\xi}}\Big{[}\exp\left(-\alpha
C(X_{t},U_{t})\right)\Big{]}}\quad,$ (85)

recovering the MPPI update rule.

Probability of Low Cost (PLC)  Similarly to [2], we can incorporate a
threshold-utility to indicate preference for costs below a given threshold,
using the indicator function: $\mathcal{L}(\tau)=\mathbbm{1}_{C\leq
C_{t,\max}}(C(X_{t},U_{t}))$. The likelihood then takes the form

$\displaystyle\mathbb{E}_{\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}},\hat{f}_{\xi}}\Big{[}\
\mathcal{L}(\tau)\Big{]}$
$\displaystyle=\mathbb{E}_{\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}},\hat{f}_{\xi}}\Big{[}\mathbbm{1}_{C\leq
C_{t,\max}}(C(X_{t},U_{t}))\Big{]}.$ (86) with the resulting gradient:
$\displaystyle\nabla_{\mathrm{\bm{\theta}}}\log\mathbb{E}_{\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}},\hat{f}_{\xi}}\Big{[}\
\mathcal{L}(\tau)\Big{]}$
$\displaystyle=\frac{\mathbb{E}_{\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}},\hat{f}_{\xi}}\Big{[}\mathbbm{1}_{C\leq
C_{t,\max}}(C(X_{t},U_{t}))\nabla_{\mathrm{\bm{\theta}}}\log\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}}\Big{]}}{\mathbb{E}_{\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}},\hat{f}_{\xi}}\Big{[}\mathbbm{1}_{C\leq
C_{t,\max}}(C(X_{t},U_{t}))\Big{]}}$ (87)

The threshold parameter $C_{t,\max}$ is set adaptively as the largest cost of
the top member in the elite fraction of sampled trajectories. Using the same
derivation for the case of Exponentiated Utility likelihood, the choice of a
Gaussian policy with a threshold-utilty reduces to the update rule for the
Cross Entropy Method [32]:

$\displaystyle\theta^{\prime}_{h}=\frac{\mathbb{E}_{\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}},\hat{f}_{\xi}}\Big{[}\mathbbm{1}_{C\leq
C_{t,\max}}(C(X_{t},U_{t}))\mathbf{u}_{h}\Big{]}}{\mathbb{E}_{\mathrm{\bm{\pi}}_{\mathrm{\bm{\theta}}},\hat{f}_{\xi}}\Big{[}\mathbbm{1}_{C\leq
C_{t,\max}}(C(X_{t},U_{t}))\Big{]}}\quad.$ (88)

##  Appendix G Nonparametric Bayesian MPC

Inference can be performed directly over the posterior of control input
sequences
$\mathrm{\bm{\theta}}\triangleq\left(\mathbf{u}_{t},\mathbf{u}_{t+1},...,\mathbf{u}_{t+H-1}\right)$.
Assuming the likelihood gradient in the Bayesian MPC setting can be derived
for differentiable dynamics:

$\displaystyle\nabla_{\mathrm{\bm{\theta}}}\log\mathbb{E}_{\hat{f}_{\xi}}\Big{[}\
\mathcal{L}(X_{t})\Big{]}$
$\displaystyle=\frac{\mathbb{E}_{\hat{f}_{\xi}}\Big{[}\mathcal{L}(X_{t})\,\nabla_{\mathrm{\bm{\theta}}}\log
p(X_{t}|\mathrm{\bm{\theta}};\xi,\mathbf{x}_{t})\Big{]}}{\mathbb{E}_{\hat{f}_{\xi}}\Big{[}\mathcal{L}(X_{t})\Big{]}}$
(89)

Similarly to the parametric formulation, the form of the cost-likelihood
$\mathcal{L}$ will result in a particular update rule. The gradient can
generally be evaluated by approximating the expectations with Monte Carlo
sampling of state trajectories given the controlled stochastic dynamics., and
evaluating the gradients on sampled trajectories. Under certain conditions,
however, it may be evaluated in closed form. Such is the case for a Linear-
Quadratic-Gaussian (LQG) system, for example [27].

##  Appendix H Motion Planning

A special case can be considered for deterministic dynamics. We can define the
stochastic dynamics using a dirac measure on the space of trajectories:
$p(X_{t}|\mathrm{\bm{\theta}};\xi,\mathbf{x}_{t})=\delta(X_{t}-\overline{X}_{t})$,
where $\overline{X}_{t}=F(\mathrm{\bm{\theta}},\xi,\mathbf{x}_{t})$, and $F$
performs consecutive application of the deterministic dynamics $f$. Setting
$\mathcal{L}(X_{t},\mathrm{\bm{\theta}}_{t})=\exp(-\alpha
C(X_{t},\mathrm{\bm{\theta}}_{t}))$, the gradient then reduces to :

$\displaystyle\nabla_{\mathrm{\bm{\theta}}}\log\mathbb{E}_{\hat{f}_{\xi}}\Big{[}\
\mathcal{L}(X_{t},\mathrm{\bm{\theta}})\Big{]}$
$\displaystyle=\frac{\nabla_{\mathrm{\bm{\theta}}}\int\exp\left(-\alpha
C(X_{t},\mathrm{\bm{\theta}})\right)\delta(X_{t}-\overline{X}_{t})dX_{t}}{\int\exp\left(-\alpha
C(X_{t},\mathrm{\bm{\theta}})\right)\delta(X_{t}-\overline{X}_{t})dX_{t}}$
(90) $\displaystyle=\frac{\nabla_{\mathrm{\bm{\theta}}}\exp\left(-\alpha
C(\overline{X}_{t},\mathrm{\bm{\theta}})\right)}{\exp\left(-\alpha
C(\overline{X}_{t},\mathrm{\bm{\theta}})\right)}$ (91)
$\displaystyle=\frac{-\alpha\exp\left(-\alpha
C(\overline{X}_{t},\mathrm{\bm{\theta}})\right)\nabla_{\mathrm{\bm{\theta}}}C(\overline{X}_{t},\mathrm{\bm{\theta}})}{\exp\left(-\alpha
C(\overline{X}_{t},\mathrm{\bm{\theta}})\right)}$ (92)
$\displaystyle=-\alpha\nabla_{\mathrm{\bm{\theta}}}C(\overline{X}_{t},\mathrm{\bm{\theta}})$
(93)

The gradient can then be evaluated in a straightforward manner via back-
propagation on the cost function, through the dynamics.

##  Appendix I Complexity

The implementation of SV-MPC requires the computation of the kernel Gram-
matrix $K(\theta^{i},\theta^{j})\ \forall\ i,j\in(1,...,m)$ for the SVGD
update. This requires an inner-product operation for all particle pairs,
resulting in a computational complexity of $\mathcal{O}(m^{2}hd)$ (where $m$ :
number of particles, $h$ : horizon, $d$ : control dimension). However, by
exploiting structured kernels for trajectories, the scaling with respect to
the horizon can be removed by parallel computation of kernel factors.
Evaluation of a kernel factor is then a constant-time operation
($\mathcal{O}(1)$), and the overall SV-MPC complexity reduces to
$\mathcal{O}(m^{2})$. In practice a relatively low order of particles is
required: $m<<1\times 10^{3}$. The core bottleneck is typically the generation
of rollout trajectories from control samples during each iteration of MPC,
which can be performed in parallel but is linear in time with respect to the
horizon $h$. As with most MPC applications, the horizon length, number of
samples, etc. can be varied to balance accuracy with runtime complexity,
depending on the constraints of the system.

##  Appendix J Experimental Details

The environment and controller parameters used in the experiments are
summarized in tables 2 and 3, respectively. Note for Table 3: the utility
function parameter, such as $\alpha$ and elite fraction, share the same values
as those used by SV-MPC for either EU or PLC likelihood.

Planar navigation  The SV-MPC controller with exponentiated-utility (EU) is
constructed with a set of 6 to 32 particles, where the gradient of each
particle is estimated via Monte-Carlo sampling of control and state
trajectories. At each round, the best-performing particle is chosen to
generate the action using (24).

For state $\mathbf{x}_{t}=(x_{t},\dot{x}_{t})$ and control
$\mathbf{u}_{t}=\ddot{x}_{t}$, where $x_{t}$, $\dot{x}_{t}$, $\ddot{x}_{t}$
are the 2D position, velocity and acceleration, respectively, and the
$x_{goal}$ the target 2D goal position, we define the instantaneous and
terminal costs (with respect to (1)):

$\displaystyle
c(\mathbf{x}_{t},\mathbf{u}_{t})=0.5(x_{t}-x_{goal})^{\top}(x_{t}-x_{goal})+0.25\dot{x}_{t}^{\top}\dot{x}_{t}+0.2\ddot{x}_{t}^{\top}\ddot{x}_{t}$
(94) $\displaystyle
c_{\mathrm{term}}(\mathbf{x}_{t})=1000(x_{t}-x_{goal})^{\top}(x_{t}-x_{goal})+0.1\dot{x}_{t}^{\top}\dot{x}_{t}$
(95)

Manipulation  Leveraging parallel simulation provides the ability to
efficiently compute highly-resolved geometric constraints between the robot
and obstacles, eliminating the need for coarsely-defined heuristics for
collision detection (such as signed-distance fields [37]). Although the
dynamics are deterministic, the problem remains challenging since the
posterior probability distribution implies a nonlinear mapping (via inverse
kinematics) from the work-space to the sampling space.

The cost-function consists of a cost on cartesian distance-to-goal from the
end-effector, as well as a penalty on control. For state
$\mathbf{x}_{t}=(e_{t},\dot{e}_{t})$ and control $\mathbf{u}_{t}=\dot{q}_{t}$,
where $e_{t}$, $\dot{e}_{t}$, are the cartesian end-effector positions and
velocities, respectively, $\dot{q}$ the joint velocities, and the $e_{goal}$
the target 3D goal position, we define the instantaneous and terminal costs
(with respect to (1)):

$\displaystyle
c(\mathbf{x}_{t},\mathbf{u}_{t})=1(e_{t}-e_{goal})^{\top}(e_{t}-e_{goal})+0.25\dot{e}_{t}^{\top}\dot{e}_{t}+0.1\dot{q}_{t}^{\top}\dot{q}_{t}$
(96) $\displaystyle
c_{\mathrm{term}}(\mathbf{x}_{t})=5000(e_{t}-e_{goal})^{\top}(e_{t}-e_{goal})+0.1\dot{e}_{t}^{\top}\dot{e}_{t}$
(97)

Stochastic HalfCheetah  We modify the cost function to reward forward velocity
only if the agent is forward-facing, as done in [22], along with a control
penalty. Without this alteration, progress can be made fairly easily by
applying torque commands in a single direction, and ‘cart-wheeling’ the
system. For instantaneous forward velocity $v_{t}$ and body angle $\beta$,

$\displaystyle
c(\mathbf{x}_{t},\mathbf{u}_{t})=0.1\mathbf{u}_{t}^{\top}\mathbf{u}_{t}-\frac{v_{t}}{2}(1+\mathrm{sgn}(\cos\beta))$
(98)

Motion Planning  The dynamics consist of a deterministic, velocity controlled
single-integrator model on the 2D position:
$\mathbf{x}_{t+1}=\mathbf{x}_{t}+\mathbf{u}_{t}\Delta t$, where
$\mathbf{x}_{t},\mathbf{u}_{t}\in\mathbb{R}^{d\times 1}$ ($d=2$). In the
example, we use a constant-control (i.e. zero-acceleration) prior on
velocities: $\mathbf{u}_{t+1}=\mathbf{u}_{t}+\mathbf{w}_{t},\
\mathbf{w}_{t}\sim\mathcal{N}(0,\Sigma)$. We can construct the prior over
sequences by first considering the convolution of control inputs over the
planning horizon $T$:

$\displaystyle\begin{bmatrix}\mathbf{u}_{0}\\\ \mathbf{u}_{1}\\\ \vdots\\\
\mathbf{u}_{T-1}\end{bmatrix}=\begin{bmatrix}I&0&\cdots&0\\\ I&I&\cdots&0\\\
\vdots&\vdots&\ddots&\vdots\\\
I&I&\cdots&I\end{bmatrix}\begin{bmatrix}\mathbf{w}_{0}\\\ \mathbf{w}_{1}\\\
\vdots\\\ \mathbf{w}_{T-1}\end{bmatrix},$ (99)

where we assume the initial control is drawn from a zero-mean distribution:
$\mathbf{u}_{0}\sim\mathcal{N}(0,\Sigma)$. For the sequences
$U\triangleq[\mathbf{u}_{0},\mathbf{u}_{1},...,\mathbf{u}_{T-1}]^{\top}$ and
$W\triangleq[\mathbf{w}_{0},\mathbf{w}_{1},...,\mathbf{w}_{T-1}]^{\top}$, we
can write the above as:

$\displaystyle U=LW$ (100)

with $L$ defined as the lower-triangular matrix. We can then define the prior
over control sequences:

$\displaystyle U\sim p(U)=\mathcal{N}(\mathbf{0},\mathbf{\Sigma})$ (101)

with covariance $\mathbf{\Sigma}=LDL^{T}$, where
$D=\mathrm{diag}(\Sigma,\Sigma,...,\Sigma)\in\mathbb{R}^{Td\times Td}$. Note
that the precision matrix $\mathbf{\Sigma}^{-1}$ is tri-diagonal, implying a
graphical structure with Markovian dependencies. In the SV-MPC optimization,
the prior can also be interpreted as a penalty which encourages smooth state
trajectories (similarly to the GP-prior described in [37]). In the motion
planning problem, we use this multi-variate Gaussian as the prior over
particles:
$p(\mathrm{\bm{\theta}})=\mathcal{N}\left(0,\mathbf{\Sigma}\right)$.

Furthermore, the cost function defined for this problem includes a smooth
obstacle cost-map. This is generated using a bi-modal mixture of Gaussians,
with the probability of collision given by $p_{\mathrm{obs}}(x_{t})$. The cost
function is then:

$\displaystyle c(\mathbf{x}_{t})$ $\displaystyle=1\times
10^{5}\,p_{\mathrm{obs}}(x_{t})$ (102) $\displaystyle
c_{\mathrm{term}}(\mathbf{x}_{t})$
$\displaystyle=1000(x_{t}-x_{goal})^{\top}(x_{t}-x_{goal})$ (103)

Table 2: Environment Parameters Experiment |  Episode length |  Trials |  state space $(\mathbf{x}_{t}\in)$ |  control space $(\mathbf{u}_{t}\in)$ |  control limits |  time-step ($\Delta t$) |  dyn. noise $\sigma_{dyn}^{2}$
---|---|---|---|---|---|---|---
Planar Nav. |  300 |  25 |  $\mathbb{R}^{4}$ |  $\mathbb{R}^{2}$ |  $\left[-50,50\right]$ |  0.015 |  0.1
Manipulation |  150 |  24 |  $\mathbb{R}^{14}$ |  $\mathbb{R}^{7}$ |  $\left[-1,1\right]$ |  0.067 |  —
Stoch. HalfCheetah |  300 |  16 |  $\mathbb{R}^{18}$ |  $\mathbb{R}^{6}$ |  $\left[-1,1\right]$ |  0.05 |  0.25
Table 3: Experiment Controller Settings. |  Planar Nav. |  Manipulation |  Stoch. HalfCheetah
---|---|---|---
Planning Horizon ($H$) |  64 |  64 |  30
Warm-start iterations |  30 |  25 |  20
Optimization iterations per timestep |  1 |  1 |  5
Control variance ($\sigma^{2}$) |  100 |  0.25 |  1
SV-MPC |  num. particles ($m$) |  6, 12, 32 |  4 |  5
num. ctrl. samples per particle |  8 |  32 |  32
step-size ($\epsilon$) |  10 |  0.1 |  1
cost-likelihood ($\mathcal{L}(\cdot)$) |  EU |  EU |  EU, PLC
MPPI |  num. control samples |  32 |  128 |  160
inverse-temperature ($\alpha$) |  $1\times 10^{-3}$ |  0.1 |  1
CEM |  num. control samples |  32 |  128 |  160
elite fraction |  0.1 |  0.1 |  0.1

##  Appendix K Additional Results

Planar Navigation  Table 6, we include quantitative summaries of performance
across control types. The performance of the SV-MPC controller improves with
increasing particle number.

Figure 6: Statistics for planar navigation task over 25 trials (4x4 obstacle grid) Controller |  Num. of particles |  Avg. cost of success ($\times 10^{3}$) |  Success rate ($\%$)
---|---|---|---
|  32 |  20.7 |  96
SV-MPC |  12 |  21.8 |  96
|  6 |  25.6 |  84
MPPI |  — |  26.5 |  64
CEM |  — |  25.4 |  64