Model Predictive Optimized Path Integral Strategies

We generalize the derivation of model predictive path integral control (MPPI) to allow for a single joint distribution across controls in the control sequence. This reformation allows for the implementation of adaptive importance sampling (AIS) algorithms into the original importance sampling step while still maintaining the benefits of MPPI such as working with arbitrary system dynamics and cost functions. The benefit of optimizing the proposal distribution by integrating AIS at each control step is demonstrated in simulated environments including controlling multiple cars around a track. The new algorithm is more sample efficient than MPPI, achieving better performance with fewer samples. This performance disparity grows as the dimension of the action space increases. Results from simulations suggest the new algorithm can be used as an anytime algorithm, increasing the value of control at each iteration versus relying on a large set of samples.

§ INTRODUCTION

Many modern control applications benefit from modeling nonlinear dynamics of the system. Examples include vehicle collision avoidance <cit.>, aerial maneuvers <cit.>, and autonomous racing <cit.>. Model predictive control (MPC) has become a popular method to accommodate the nonlinearity of the dynamics <cit.>. Some MPC formulations use a quadratic objective function and linear constraints encoding an approximation of the dynamics. Other approaches involve solving the nonlinear program associated with the dynamics by using variants of sequential quadratic programming and interior point methods to approximate solutions <cit.>. Another technique that has emerged is the use of the Koopman operator <cit.>. <cit.> show that combining this approach with a linear MPC controller reduces the computational complexity of the underlying optimization problem to one that is comparable to a linear dynamical system of similar state and action space sizes. However, efficiently developing finite-dimensional representations of the linear dynamics is still an area of active research <cit.>.

An alternative MPC-based approach is to use Monte Carlo sampling as part of the optimization of the trajectory. Model predictive path integral control (MPPI) is one such approach, which has been motivated from the perspective of the Hamilton-Jacobi-Bellman equation <cit.>, information theory <cit.>, and stochastic optimization <cit.>. MPPI can work with arbitrary system dynamics and cost functions, and it uses importance sampling to estimate the optimal control input. <cit.> demonstrated MPPI to work in real time with a neural network representing the system dynamics. The main idea behind MPPI is to sample a set of control sequences, compute the cost of each sequence, and then calculate a cost informed weighted average of the sampled controls. The first control in the sequence is executed and the remainder of the weighted average is used as the mean of the proposal distributions at the next time step.

The insight that the performance of MPPI is tightly coupled with sampling from a proposal distribution defined from the previous control sequence has led to research in different areas. One focus has been on increasing the robustness when a disturbance changes the state of the system. Such a disturbance often results in samples from the proposal distribution, which is based on the previous control sequence, to not accurately estimate the the optimal control. <cit.> augment MPPI with an ancillary controller which tracks the nominal trajectory forming Tube-MPPI. <cit.> further extend this approach and provide bounds on the free energy growth while rejecting unknown disturbances. This methodology still relies on developing a nominal control input from sampling separate distributions at each time step around the previous control sequence.

The formulation of MPPI structured the importance sampling to be from separate distributions at each control step. This formulation limits the form of the proposal distribution. In execution, we are limited to a finite number of samples and the quality of the estimate using importance sampling is influenced by the reference distribution <cit.>. This limitation often requires large sample sizes to approximate the optimal control and manifests in experiments when the dimension of the action space increases <cit.>.

<cit.> extend MPPI by adding an informed sampling process. They use conditional variational autoencoders to learn distributions that imitate samples from a training data set containing optimized controls. Using these learned distributions, they adjust the mean of the sampling distribution before executing the MPPI process. <cit.> use optimization methods originally developed for gradient descent to adjust the control input at each time step, increasing the convergence to the optimal control. <cit.> introduce a Bayesian MPC approach that allows for multi-modal distributions and approximate the posterior distribution using Stein variational gradient descent. <cit.> implement an iteration step within the MPPI framework, but only adjust the mean of the sampling distributions from the weighted average of the previous MPPI iteration. <cit.> use the Tsallis divergence to form a general sampling based algorithm and use a similar iteration procedure to refine the distribution within the control update step. However, the sampling and optimization are over separate distributions at each control step.

These works either adjust the control output after the sampling, adjust only the mean of the proposal distribution, or update individual distributions corresponding to the control at each time step. Our work extends these ideas by providing a method to further refine the proposal distribution by learning a more suitable mean and covariance after obtaining feedback from an original set of samples. To accomplish this change, we generalize the derivation of MPPI to allow for a single joint distribution across controls in the control sequence. With this formulation, we add an adaptive importance sampling (AIS) procedure at each time step. The other approaches discussed can be implemented in addition with our proposed reformulation and benefit from the introduction of an AIS step within the MPPI process. Adapting the proposal distribution across control inputs results in an increase in performance with fewer samples, albeit with a reduction in parallel capability. Our work allows for the MPPI algorithm to be implemented with smaller sample sizes and as an anytime algorithm, increasing performance as computation time allows.

§ MODEL PREDICTIVE OPTIMIZED PATH INTEGRAL CONTROL

In this section, we generalize MPPI to treat the controls across a time horizon as a single input. We then discuss modifications outlined in other work with MPPI, present the model predictive optimized path integral (MPOPI) control algorithm, and then comment on the computational cost.

§.§ Generalized Problem Formulation and Derivation

In MPPI, the control sequence is treated as a set of distinct controls at time $t$ with the inputs to the system as $\bv_t\sim\mathcal{N}(\bu_t,\Sigma)$ where $\bu_t$ is the commanded control input at time $t$ and $\Sigma$ is the covariance of the Gaussian noise associated with the input. Therefore, when sampling from the control distribution and estimating the optimal control, we are sampling from separate distributions corresponding to each time step. However, the weights derived from importance sampling are dependent on the cost of the whole control sequence. Using separate distributions at each time step presents a problem if we want to improve our estimation of the expectation by integrating techniques that use the calculated trajectory costs and the control sequence as a single input. Our solution is to treat the sequence of controls as one control input and construct a combined covariance matrix and sample from one, larger joint distribution. This formulation allows different optimization techniques to be applied between time steps to obtain a better estimate of the optimal control through adaptive importance sampling.

Mirroring the derivation of <cit.>, we consider the same dynamical system, $\bx_{t+1} = \mathbf{F}(\bx_t, \bv_t)$, where $\bx_t \in \mathbb{R}^n$ is the state of the system at time $t$, $\bv_t \in \mathbb{R}^m$ is the input to the system, and $\mathbf{F}$ is the state-transition function of the system. The input, $\bv_t$, is a random vector generated by a white-noise process with density function $\bv_t \sim \mathcal{N}(\bu_t, \Sigma_t)$ where $\bu_t \in \mathbb{R}^m$ is the commanded control input at time $t$ and $\Sigma_t$ is the covariance of the Gaussian noise associated with input $\bu_t$. We will define the vector $\bU$ to be composed of the components of each control input $\bU = \left[\bu_0, \bu_1, \dots, \bu_{T-1}\right], \bU \in \mathbb{R}^{mT}$ and define the combined covariance matrix as

\begin{align} \label{eq:big_sigma}
\footnotesize{
\bSigma =
\begin{bmatrix}
\Sigma_0 &  \textbf{0} &   \dots &   \textbf{0} \\
\textbf{0} &   \Sigma_1 &   \dots &   \textbf{0} \\
\vdots &   \vdots &   \ddots &  \vdots \\
\textbf{0} &   \textbf{0} &   \dots &   \Sigma_{T-1}
\end{bmatrix}, \bSigma \in  \mathbb{R}^{mT \times mT}.}
\end{align}

Therefore, the input, $\bV$, is a random vector with density function $\bV \sim \mathcal{N}(\bU, \bSigma)$ and we can express the analytical form of the density function $q$ corresponding to the distribution $\mathbb{Q}_{\bU,\bSigma}$ as

\begin{align}
\small{
q(\bV \mid \bU, \bSigma) = ((2
& \small{
\pi)^{mT}|\bSigma|)^{-1/2} \nonumber
} \\
& \small{
\exp\left(-\frac{1}{2}(\bV-\bU)^\intercal\Sigma^{-1}(\bV-\bU)\right).
\end{align}

Similar to <cit.>, we define our cost function as one that can be decomposed into a state dependent cost $c(\bX)$ and a quadratic control cost $\mathcal{L}(\bX,\bU) = c(\bX) + \frac{\lambda}{2}(\bU^\intercal\bSigma^{-1}\bU + \beta_{\Tilde{\bU}}^\intercal\bU) + c_{\Tilde{\bU}}$ where $\bX = [\bx_0, \textbf{F}(\bx_0, \bv_0), \textbf{F}(\textbf{F}(\bx_0, \bv_0), \bv_1),\dots]$, $\bX \in \mathbb{R}^{nT}$, $\bv_t \in \mathbb{R}^m$ are the components of $\bV~\in~\mathbb{R}^{mT}$, and $\beta_{\Tilde{\bU}}$ and $c_{\Tilde{\bU}}$ are constants. We can now write our optimal control problem as

\begin{equation} \label{eq:opt_cont_prob}
\small{
\bU^* = \argmin_{\bU \in \mathcal{U}} \mathbb{E}_{\mathbb{Q}_{\bU,\bSigma}}\left[ \phi(\bX) + \mathcal{L}(\bX, \bU) \right]
\end{equation}

where $\mathcal{U}$ is the set of all valid control vectors and $\phi(\bX)$ is a terminal cost function operating on the terminal state within $\bX$. Based on our definition of $\bX$, the state cost of a control sequence is $S(\bV) = c(\bX) + \phi(\bX)$ and the free-energy of the control system is then

\begin{align}
\small{
\mathcal{F}(S,\,} &
\small{
p,\bX,\lambda) = -\lambda \log \left( \mathbb{E}_{\mathbb{P}} \left[ \exp \left( -\frac{1}{\lambda} S(\bV)\right)\right]\right)
} \\
\small{ = -\lambda \log \left( \mathbb{E}_{\mathbb{Q}_{\bU,\bSigma}} \left[ \exp \left( -\frac{1}{\lambda} S(\bV) \right) \frac{p(\bV)}{q(\bV\mid\bU,\bSigma)} \right]\right) \label{eq:free-energy_q}
\end{align}

where $\lambda$ is the inverse temperature and $\mathbb{P}$ is the distribution over a set of control vectors with probability density, $p$. From the concavity of the logarithm function, we can apply Jensen's inequality and then use the definition of the KL divergence

\begin{equation}
\small{
\mathcal{F}(S,p,\bX,\lambda) \leq \mathbb{E}_{\mathbb{Q}_{\bU,\bSigma}} \left[ S(\bV) \right] + \lambda\mathbb{D}_{KL}(\mathbb{Q}_{\bU,\bSigma} \mid\mid \mathbb{P}). \label{eq:free_energy}
\end{equation}

This inequality relates the free energy to the state cost of the control problem and the divergence between distribution $\mathbb{Q}_{\bU,\bSigma}$ and $\mathbb{P}$.

Suppose the base distribution is $p(\bV) = q(\bV \mid \Tilde{\bU}, \bSigma)$ where $\Tilde{\bU}$ is a nominal input applied to the system. Defining, $\beta_{\Tilde{\bU}}^\intercal = -2\Tilde{\bU}^\intercal\bSigma^{-1}$ and $c_{\Tilde{\bU}} = \Tilde{\bU}\bSigma^{-1}\Tilde{\bU}$, we have

\begin{align}
\small{
\mathbb{D}_{KL}(\mathbb{Q}_{\bU,\bSigma} \mid\mid\,}
& \small{
\mathbb{Q}_{\Tilde{\bU},\bSigma})
= \mathbb{E}_{\mathbb{Q}_{\bU,\bSigma}} \left[ \log \left( \frac{q(\bU \mid \bSigma)}{q(\Tilde{\bU} \mid \bSigma)}  \right)   \right]
} \\
& \small{
= \mathbb{E}_{\mathbb{Q}_{\bU,\bSigma}} \left[ \frac{1}{2} \left( (\bU - \Tilde{\bU})^\intercal \bSigma^{-1} (\bU - \Tilde{\bU}) \right) \right]
} \\
\small{
= \mathbb{E}_{\mathbb{Q}_{\bU,\bSigma}} \left[ \frac{1}{2} \left( \bU^\intercal\bSigma^{-1}\bU + \beta_{\Tilde{\bU}}^\intercal\bU + c_{\Tilde{\bU}} \right)  \right]. \IEEEyesnumber\label{eq:kld_law}
\end{align}

Using <ref> and the state cost, we can rewrite <ref> as

\begin{align}
\small{
\mathcal{F}(S,p,\bX,\lambda) }
\small{
\leq \mathbb{E}_{\mathbb{Q}_{\bU,\bSigma}} \left[ \phi(\bX) + c(\bX) \phantom{\frac{1}{2}} \right. \nonumber
} \\
& \small{
\qquad \qquad \quad \left. + \frac{\lambda}{2} \left( \bU^\intercal\bSigma^{-1}\bU + \beta_{\Tilde{\bU}}^\intercal\bU + c_{\Tilde{\bU}} \right) \right]
} \\
& \small{
\leq \mathbb{E}_{\mathbb{Q}_{\bU,\bSigma}} \left[ \phi(\bX) + \mathcal{L}(\bX, \bU) \right]
\end{align}

and we have established that the free energy provides a lower bound on our reformulated optimal control problem. Furthermore, this inequality becomes an equality if we consider the optimal control distribution $\mathbb{Q}^*$ defined by its density function $q^*(\bV) = \frac{1}{\eta}\exp\left(-\frac{1}{\lambda}S(\bV) \right)p(\bV)$. We can now minimize the divergence between our control distribution and the optimal distribution and achieve an optimal control trajectory instead of minimizing <ref> directly.

The processes of minimizing the KL-divergence between our control distribution and the optimal distribution also follows closely to the original derivation <cit.> and results in the quadratic minimization problem

\begin{equation}
\small{
\bU^* = \argmin_{\bU\in\mathcal{U}} \left( \mathbb{E}_{\mathbb{Q}^*} \left[ (\bV-\bU)^{\intercal}\bSigma^{-1}(\bV-\bU)  \right]  \right)
\end{equation}

with the optimal solution in the unconstrained case of

\begin{equation} \label{eq:opt_unconstrained}
\small{
\bU^* = \mathbb{E}_{\mathbb{Q}^*} \left[ \bV \right].
\end{equation}

Previous work has addressed incorporating control constraints as part of the dynamical model, therefore we only consider the unconstrained solution and discuss modifications in <ref>.

Using importance sampling to determine the expectation in <ref> also follows the original derivation closely. Let $\mathbb{Q}_{\hat{\bU}, \bSigma}$ represent a tentative sampling distribution to perform importance sampling from, then we get our control through

\begin{gather}
\small{
\bU^* = \mathbb{E}_{\mathbb{Q}_{\hat{\bU}, \bSigma}} \left[ w(\bV)\bV  \right]
}\label{eq:opt_expec} \\
% \end{equation}%
% \begin{equation} \label{eq:weight}
\small{
w(\bV) = \frac{1}{\eta}\exp\left(-\frac{1}{\lambda}\left(S(\bV) + \lambda (\hat{\bU} - \Tilde{\bU})^\intercal\bSigma^{-1}\bV \right) \right)
} \label{eq:weight} \\
% \end{equation}%
% \begin{equation}
\small{
\eta = \int \exp\left(-\frac{1}{\lambda}\left(S(\bV) + \lambda (\hat{\bU} - \Tilde{\bU})^\intercal\bSigma^{-1}\bV\right)\right)d\bV.
\end{gather}

No assumptions were changed and all of the theoretical results from MPPI still hold such as <ref> being a global optimal solution (from the information-theoretic perspective) under the assumption we can evaluate the expectation perfectly. With no other modification, this approach is equivalent to traditional MPPI. When constructing $\bSigma$ in <ref> we only allowed for control input noise in order to mirror MPPI. In the derivation, we made no assumptions on the structure of $\bSigma$ except that it is a valid covariance matrix and $\bSigma \in \mathbb{R}^{mT \times mT}$.

§.§ Other Modifications

Information-theoretic model predictive control does present some practical issues such as numerical stability with the trajectory costs. Previous work established methods to address such problems and we extend those concepts to our algorithm. Notice in <ref> that $\lambda$ directly affects the control cost. That is, as $\lambda$ increases and distributes the weight among trajectories, it also increases the contribution of the control cost. We can decouple the influence of $\lambda$ by redefining the base distribution as $\Tilde{p}(\bV) = p(\bV \mid \alpha\hat{\bU}, \bSigma)$ where $0 \leq \alpha \leq 1$. The new base distribution changes our weight to

\begin{equation} \label{eq:weight-g}
\small{
w(\bV) = \frac{1}{\eta}\exp\left(-\frac{1}{\lambda}\left(S(\bV) + \gamma (\hat{\bU} - \Tilde{\bU})^\intercal\bSigma^{-1}\bV \right) \right)
\end{equation}

where $\gamma = \lambda(1-\alpha)$. With this change, as $\alpha$ moves from 0 to 1, the control cost is less of a factor in the overall weight. Additionally, calculating the weight in <ref> can result in numerical instability. To avoid those issues a normalizing factor is introduced to set the lowest cost to zero.

The control sequence produced by the sampling methodology can produce inputs that change rapidly across time steps. There are a few ways to address this case. One option is to incorporate a smoothing process into the dynamics model. Since there are no restrictions on the model dynamics in the formulation, we can incorporate different input modifications as part of the dynamics. This idea follows the same concept on how to integrate control constraints allowing us to only consider the unconstrained solution in <ref>. We can modify our inputs by introducing a function $g$ that applies the appropriate smoothing and control constraints to our inputs before applying the dynamics and our system becomes $\bx_{t+1} = \textbf{F}(\bx_t, g(\bv_t))$.

§.§ Adaptive Importance Sampling

Using importance sampling to estimate <ref> is a key aspect of MPPI. However, since we are limited to a finite number of samples, the quality of the estimate is influenced by the reference distribution <cit.>. One approach to improve our estimate is to use AIS to adapt our proposal distribution iteratively. There are many AIS algorithms with different performance costs and benefits as surveyed by <cit.>. Control problems present various challenges and the choice of which AIS algorithm to implement will vary across scenarios. With the generalized formulation, we are not restricted to the choice of an AIS algorithm as $\bU$ and $\bSigma$ can be updated to achieve a better proposal distribution including correlation terms across control steps. Separating the AIS step from the MPPI algorithm also allows different parameter settings for the control problem and the AIS algorithm (e.g. different $\lambda$ values to allow exploration during AIS but more selective when calculating the final control). This work focuses on the integration of different AIS algorithms into MPPI and leaves the investigation of specific trade-offs of different algorithms for future research.

§.§ Algorithm

<Ref> outlines the MPOPI algorithm. This algorithm is executed at each time step until the task is complete. Bold capital letters (e.g. $\bU, \bSigma, \bX)$ represent the composite vectors/matrices as described in <ref> and bold lowercase letters with subscripts (e.g. $\bu_t, \bx_t$) represent the components of the composite vectors. The call to PerformAIS represents a call to an AIS algorithm to execute at each iteration. One of the key differences from MPPI to note is the calculation of the control cost component on <ref> and the weighted average to update $\bU$ (<ref>). We have to account for the appropriate change in control amount as we change our proposal distribution. If we set $L = 1$, then $\bU^\prime - \bU$ vanishes and MPOPI is equivalent to MPPI.

Model Predictive Optimized Path Integral Control
$\textbf{F}, g, K, T$
$\bU \in \mathbb{R}^{mT}$
$\bSigma \in \mathbb{R}^{mT \times mT}$
$\phi, c, \lambda, \alpha$ *[f]Cost function/parameters
$L$ *[f]Max number of AIS iterations
$\psi_1, \psi_2, \dots , \psi_p$ *[f]AIS parameters

$\bx_0 \gets \text{GetStateEstimate}()$

$\bU^\prime \gets \bU$

$\bSigma^\prime \gets \bSigma$

$\ell \gets 1 $ to $L$
$k \gets 1$ to $K$
Sample $\mathbfcal{E}_k$ from $\mathcal{N}(0, \bSigma^\prime)$

$t \gets 1$ to ${T}$
$\bx_t \gets \textbf{F}(\bx_{t-1}, g(\bu^\prime_{t-1} + \boldsymbol\varepsilon_{t-1}^k))$

$s_k \gets c(\bX) + \phi(\bX)  + \lambda(1 - \alpha) {\bU^{\prime}}^\intercal\bSigma^{-1}\left( \mathbfcal{E}_k + \bU^\prime - \bU \right)$

$\ell < L$$\bU^\prime, \bSigma^\prime \gets      \text{PerformAIS}(\bU^\prime, \bSigma^\prime, \textbf{S}, \psi_1, \psi_2, \dots, \psi_p, \ell)$

$\rho \gets \min(\textbf{S})$

$\eta \gets \sum_{k=1}^K \exp\left( -\frac{1}{\lambda}(s_k - \rho)\right)$

$k \gets 1$ to $K$
$w_k \gets \frac{1}{\eta}\exp\left( -\frac{1}{\lambda}(s_k - \rho)\right)$

$\bU \incr w_k\left( \mathbfcal{E}_k + \bU^\prime - \bU  \right)$

$t \gets 1$ to ${T}-1$
$\bu_{t-1} \gets \bu_t$

$\bu_{T-1} \gets \text{Initialize}(\bu_{T-1})$

<Ref> provides a graphical depiction of how MPOPI can improve samples to better estimate the optimal control. The trajectories were generated from throttle and steering commands for controlling a car. <Ref> depicts the trajectories of the controls sampled from the initial proposal distribution. After each iteration, the proposal distribution is updated and new samples are generated. <Ref> and <ref> show how the sampled controls evolve, resulting in higher scoring trajectories after each iteration of the AIS algorithm.

§.§ Computational Cost

A key enabler for MPPI to work real time is its ability to be implemented in parallel. The computational complexity of MPPI can be summarized as the complexity of obtaining $KT$ samples and the complexity of propagating the system dynamics $T$ times for $K$ samples in parallel. The addition of AIS changes this complexity. The sampling and propagation of the system dynamics must occur $L$ times, along with the addition of the complexity of the AIS algorithm $L-1$ times. Modern sampling and AIS algorithms are efficient. The largest computational burden comes from propagating the dynamics in $L$ sequential steps. Using AIS will allow the use of fewer samples and still take advantage of parallel operations during the propagation step, but at the burden of reestablishing the parallel computations $L$ times.

The MPOPI algorithm is more sample efficient and the iterative nature lends itself to an anytime approach where $L$ can increase with computation time available. However, these benefits come at the cost of a reduction in parallel capability. For example, given $M=KL$ samples, MPPI would propagate the system dynamics in parallel for all $M$ samples. However, MPOPI requires propagating $K$ samples in parallel in $L$ sequential steps where the execution of the AIS algorithm must occur after each propagation of $K$ samples. The complexity of establishing parallel computations is hardware, problem, and implementation specific. As problem complexity increases, MPOPI provides an alternative approach to adding more samples.

Initial trajectories.

Trajectories after AIS iteration $3$.

Trajectories after AIS iteration $8$.

Evolution of $150$ trajectories from sampled controls using MPOPI and the cross entropy method for the AIS algorithm. Green depicts high valued trajectories and red represents low valued trajectories within each sub-figure. The control is a series of throttle and steering commands. Details of this environment are described in <ref>.

§ EXPERIMENTS

We tested the MPOPI algorithm in different virtual environments and compared it to MPPI with the same modifications discussed in <ref>. MPPI has been shown across the literature to perform equitably or outperform numerous other control algorithms and was chosen as the baseline <cit.>. We integrated five different AIS algorithms with MPOPI. The algorithms have the ability to use multiple proposal distributions, but we chose to use one proposal distribution to keep the comparison closer to MPPI. We implemented a Population Monte Carlo (PMC) algorithm <cit.>, a mean only moment matching AIS algorithm ($\mu$-AIS), a mean and covariance moment matching AIS algorithm ($\mu\Sigma$-AIS) similar to Mixture-PMC <cit.>, a Cross-Entropy method (CE) <cit.>, and a Covariance Matrix Adaptation Evolutionary Strategy (CMA) <cit.>. The $\mu$-AIS algorithm is similar to the iterative approach introduced by <cit.>, but we allowed for different values of $\lambda$ for updating the mean and the final weight calculation of the samples. This separation resulted in better performance versus using a shared $\lambda$. We kept parameters constant across similar algorithms to show a more direct comparison. Reference the repository for parameters used and details of the implementation.

We first looked at the continuous version of the MountainCar environment <cit.> and then performed simulations with a non-linear car model. To increase the action space size and complexity, we modified the car environment to control multiple cars cooperatively and conducted experiments on MuJoCo gym environments. In this section, the term effective samples refers to the total samples an algorithm had available. For MPPI, it is the number of samples and for MPOPI, it is the number of samples multiplied by the number of AIS iterations, $KL$. We also discuss rewards when describing objective functions instead of costs. The effective sample sizes were increased by changing the number of samples or AIS iterations with MPOPI or the total number of samples for MPPI. The smallest sample size in the MountainCar and car racing scenarios restricted MPOPI to one AIS iteration to show the equivalence to MPPI and provide a more direct comparison. All simulations used the same parameters for MPPI and MPOPI (e.g. $\lambda$, $\alpha$, $T$).

§.§ MountainCar Environment

The goal of the MountainCar problem is to get an under-powered car up a hill to a goal location with the dynamics and parameters as specified by <cit.>. The action space consisted of a single continuous action at each time step. The reward function for this problem was modified to be $R(x_t, v_t) = -1 +|v_t| + 100000R^*$ where $R^*$ is an indicator variable for reaching the goal location with $v_t>0$. The environment terminated when the car reached the goal location or at 200 steps. The simulations consisted of 1000 trials for effective samples ranging from 20 to 180 with MPOPI using 20 samples and varying the number of AIS iterations. The results are shown in <ref>. The algorithms converge to a similar number of steps while also reaching a point where increasing the number of samples provided little return. MPPI was able to reach similar performance but failed to outperform versions of MPOPI except for PMC. The CE and CMA versions of MPOPI were able to reach the plateau with as few as 40 and 60 effective samples respectively. The PMC algorithm uses multinomial resampling and struggles with a small initial sample size.

MountainCar results. Shaded regions are 95% confidence intervals. Adjusting the proposal distribution allows MPOPI to achieve MPPI level performance in as few as 2 iterations and 40 effective samples.

§.§ Car Racing

This scenario simulated a form of car racing and was constructed to be similar to the virtual racing environment used by <cit.>. The total length of the track was approximately 1.18 with multiple curves. We implemented a nonlinear single-track model that captures transient dynamics in corning with a variant of a Fiala brush tire model <cit.>. The model was a representation of the electric X1, an autonomous experimental car. For details of the model dynamics and parameters of the X1 see <cit.>. The action space for a single car consisted of two continuous inputs per time step: a steering command and a throttle/brake command. The controls were issued at 10 with the dynamics operating at 100. The reward function for each car was $R(\bx_t) = 2|v_t| - |d| - 5000R^*_\beta - 1000000R^*_t$ where $v_t$ is the velocity of the car at time step $t$, $d$ is the distance of the car from the middle of the track lane, $R^*_\beta$ is an indicator variable for exceeding a certain drift amount $\beta$, and $R^*_t$ is an indicator variable for when the car leaves the track boundary ($\beta$ is the angle between the velocity vector and the longitudinal axis; the limit was set to 45). All runs consisted of $25$ trials and each trial concluded when the car completed $2$ laps, exceeded the $\beta$ limit for more than 5, or was outside the track boundary for greater than 1. The number of effective samples varied from $375$ to $2250$ with MPOPI using iterations of $150$ or $375$ samples depending on the number of cars and the AIS algorithm. More details of this environment and videos can be seen in the repository.

Results of controlling a single car are shown in <ref>. There were no violations of the track boundary or the $\beta$ limit. All versions of MPOPI except for $\mu$-AIS outperformed MPPI and achieved MPPI peak performance with as few as $375$ samples and $3$ iterations of the AIS algorithm. There was more of a disparity in performance with fewer effective samples. The single car scenario resulted in the car attaining speeds of 39/ with $\beta$ values exceeding 34.

Single Car.

Two Cars.

Three Cars.

Car racing results. Shaded regions are 95 confidence intervals. The sample efficiency of MPOPI versus MPPI becomes more pronounced as the problem complexity increases.

We modified the car racing environment to cooperatively control multiple cars. For $N_c$ cars, the reward function was

\begin{equation}
\small{
R(\bx_t) = \sum_{i=1}^{N_c} R_i - 11000R^*_c - \sum_{i=1}^{N_c-1}\sum_{j=i+1}^{N_c} |d_{i,j}|
\end{equation}

where $R_i$ is the individual reward for car $i$, $R^*_c$ is an indicator for the cars being within $4$ m of each other, and $d_{i,j}$ is the distance between car $i$ and car $j$. The last term was added to increase complexity by emphasizing coordinated actions across the action space.

Results for two and three cars are shown in <ref> and <ref>. There were no violations for the two and three car scenarios. The success rate of a trial in the 4-car scenario with no violations is shown in <ref>. For a 5-car scenario, MPPI was only able to complete $3$ out of $25$ trials with $6000$ samples. The CE and CMA versions of MPOPI were able to control up to six cars successfully with as few as $1500$ effective samples.

For a given effective sample size, the CE and CMA methods benefited from using fewer samples with more iterations as the number of cars increased. However, as the sample size decreased, the method to approximate the covariance matrix in the CE version of MPOPI became more important. Algorithms specializing in estimating large covariance matrices with few samples resulted in higher performance. Details of the different covariance matrix estimation algorithms tested are provided in the repository. Changing the sample size and iteration count on the single and two car scenarios did not have a noteworthy change in performance.

4-Car completion rate with zero violations.

21.0cmEffective Samples     2*MPPI     2*$\mu$-AIS     2*$\mu\Sigma$-AIS     2*PMC      2*CE      2*CMA

$750$      $0.67$     $0.96$     $0.88$     $0.80$     $1.00$     $0.96$

$1125$     $0.84$     $1.00$     $0.88$     $0.84$     $1.00$     $1.00$

$1500$     $0.88$     $1.00$     $0.96$     $0.96$     $1.00$     $1.00$

$1875$     $0.88$     $1.00$     $1.00$     $1.00$     $1.00$     $1.00$

$2250$     $0.96$     $1.00$     $1.00$     $1.00$     $1.00$     $1.00$

§.§ MuJoCo Gym Environments

To further test MPOPI, we conducted experiments on MuJoCo gym environments <cit.> using EnvPool <cit.>. No environmental parameters were changed from the documented implementations. We ran each scenario $10$ times and the experiments were conducted for $250$ steps. We conducted experiments using only the CE version of MPOPI based on previous results. The average total accumulated reward is shown for two tasks in <ref>. Algorithm parameters were not tuned for performance on the different scenarios and were kept consistent across MPPI and MPOPI. Reference the repository wiki page for more details on the parameters of the algorithms used.

HalfCheetah-v$4$ and Ant-v$4$ have six and eight dimensional action spaces respectively. These environments were chosen to test MPOPI on different scenarios with complex control spaces. Similar to previous results, MPOPI achieved similar performance as MPPI with fewer effective samples. The MuJoCo gym and multi-car results demonstrate the significance the proposal distribution can have on the quality of the importance sampling estimate of the optimal control. MPOPI increases in performance with each iteration of the AIS algorithm and also increases in performance with a larger initial sample size. These results suggest MPOPI can be used in complex problems as an anytime algorithm where the sample size and iteration count can be adjusted as computation time allows.

MuJoCo environment results. Average total reward for $250$ steps on the HalfCheetah-v$4$ and Ant-v$4$ environments. The margin shown is the 95 confidence interval.

21.0cmEffective Samples     2cHalfCheetah-v4         2cAnt-v4

2-3 5-6
MPPI     MPOPI-CE        MPPI     MPOPI-CE

$250$     $1554 \pm 224$     $2154 \pm 223$        $1444 \pm \phantom{0}92$     $1711 \pm 161$

$500$     $1973 \pm 162$     $2771 \pm 183$        $1635 \pm 147$     $1839 \pm \phantom{0}93$

$1000$     $2071 \pm 103$     $3358 \pm 328$        $1814 \pm 100$     $2197 \pm \phantom{0}89$

$1500$     $2192 \pm 197$     $3798 \pm 140$        $1946 \pm 131$     $2251 \pm \phantom{0}77$

$3000$     $2440 \pm 108$     $4737 \pm 315$        $2180 \pm \phantom{0}95$     $2417 \pm 114$

§ CONCLUSION

We increased the sample efficiency of MPPI by implementing adaptive importance sampling into the original importance sampling step of MPPI. The benefit of learning a better proposal distribution from previous samples was demonstrated in simulated environments and became further evident as the dimension of the action space increased. Simulation results suggest MPOPI can be used as an anytime algorithm, increasing the value of control at each iteration versus relying on a single large set of samples. The modification to adjust the proposal distribution to increase the efficiency and quality of the samples used is a promising change to an already high performing control algorithm.

Missing 'biblatex' package
The bibliography requires the 'biblatex' package.

journaltitleIEEE Transactions on Vehicular Technology
titlePath planning and tracking for vehicle collision avoidance based on model predictive control with multiconstraints

journaltitleAerospace Science and Technology
titleNonlinear control of aerial manipulation systems

journaltitleAutonomous Robots
titleAlphaPilot: autonomous drone racing

journaltitleInternational Journal of Advanced Manufacturing Technology
titleReview on model predictive control: an engineering perspective

journaltitleVietnam Journal of Mathematics
titleRecent Advances in Quadratic Programming Algorithms for Nonlinear Model Predictive Control

journaltitleJournal of Nonlinear Science
titleA Data–Driven Approximation of the Koopman Operator: Extending Dynamic Mode Decomposition

titleLinear predictors for nonlinear dynamical systems: Koopman operator meets model predictive control

booktitleAmerican Control Conference (ACC)
titleA Class of Logistic Functions for Approximating State-Inclusive Koopman Operators

journaltitleJournal of Machine Learning Research
titleA Generalized Path Integral Control Approach to Reinforcement Learning

booktitleIEEE International Conference on Robotics and Automation (ICRA)
titleInformation theoretic MPC for model-based reinforcement learning

booktitleIEEE Conference on Decision and Control (CDC)
titleRelative entropy and free energy dualities: Connections to Path Integral and KL control

booktitleIEEE International Conference on Robotics and Automation (ICRA)
titleConstrained Sampling-based Trajectory Optimization using Stochastic Approximation

journaltitleIEEE Transactions on Robotics
titleInformation-Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving

booktitleRobotics: Science and Systems
titleRobust Sampling Based Model Predictive Control with Sparse Objective Information

journaltitleIEEE Robotics and Automation Letters
titleRobust Model Predictive Path Integral Control: Analysis and Performance Guarantees

journaltitleAnnals of Operations Research
titleA tutorial on the cross-entropy method

Georgia Institute of Technology

titleModel predictive path integral control: Theoretical foundations and applications to autonomous driving

booktitleIEEE International Conference on Robotics and Automation (ICRA)
titleInformed Information Theoretic Model Predictive Control

booktitleIEEE International Conference on Robotics and Automation (ICRA)
titleAcceleration of Gradient-Based Path Integral Method for Efficient Optimal and Inverse Optimal Control

booktitleConference on Robot Learning
seriesProceedings of Machine Learning Research
titleStein Variational Model Predictive Control

booktitleAIAA SciTech Forum
titleFlying complex maneuvers with model predictive path integral control

booktitleRobotics: Science and Systems
titleVariational Inference MPC using Tsallis Divergence

journaltitleIEEE Signal Processing Magazine
titleAdaptive Importance Sampling: The past, the present, and the future

Taylor     Francis

journaltitleJournal of Computational and Graphical Statistics
titlePopulation Monte Carlo

journaltitleStat and Comput
titleAdaptive importance sampling in general mixture classes

titleThe cross-entropy method: a unified approach to combinatorial optimization, Monte-Carlo simulation, and machine learning

journaltitleIEEE Signal Processing Letters
titleRobust Covariance Adaptation in Adaptive Importance Sampling

University of Cambridge

titleEfficient Memory-Based Learning for Robot Control

MIT Press

titleAlgorithms for Decision Making

journaltitleIEEE Transactions on Intelligent Transportation Systems
titleImpacts of Model Fidelity on Trajectory Optimization for Autonomous Vehicles in Extreme Maneuvers

journaltitleIEEE Transactions on Intelligent Transportation Systems
titleCoordinating Tire Forces to Avoid Obstacles Using Nonlinear Model Predictive Control

booktitleIEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)
titleMuJoCo: A physics engine for model-based control

titleOpenAI Gym

journaltitlearXiv preprint arXiv:2206.10558
titleEnvPool: A Highly Parallel Reinforcement Learning Environment Execution Engine