Cross Entropy Method MPC

Topics: Model Predictive Control

Concept

Cross Entropy Method Model Predictive Control (CEM-MPC) is a sampling-based MPC algorithm. It consists of sampling multiple trajectories of control sequences from a control input distribution, rolling out the dynamics, computing the cost associated with each trajectory, then updating the control input distribution based on the best or elite trajectories. So intuitively, it's kind of a Monte Carlo / evolutionary method of solving MPC.

Algorithm

Following the Finite Horizon Optimal Control MPC formulation, the control sequence is defined as

$$U = \begin{bmatrix} u_0 \\ u_1 \\ \vdots \\ u_{N-1} \end{bmatrix} \in \mathbb{R}^{N d_u}$$

where each $u_i \in \mathbb{R}^{d_u}$.

CEM-MPC maintains a probability distribution over $U$, usually Gaussian

$$q_{\theta}(U) = \mathcal{N}(U; \mu, \Sigma)$$

where

$$\mu = \begin{bmatrix} \mu_0 \\ \mu_1 \\ \vdots \\ \mu_{N-1} \end{bmatrix} \;\;, \quad \Sigma=diag(\Sigma_0, \, ... \, ,\Sigma_{N-1})$$

At time $t$, the initial distribution of CEM-MPC is initialized

$$q_{\theta^{(0)}}(U) = \mathcal{N}(U; \mu^{(0)}, \Sigma^{(0)})$$

Then for CEM iteration $k=0, \, ... \, K_{CEM-1}$, the following step are taken:

1. Sample $M$ candidate control sequences
   $U^{(m,k)} \sim q_{\theta^{(k)}}(U)$
2. Roll out the dynamics using the model
3. Compute the cost associated with each trajectory $J^{(m,k)} = \sum_{i=0}^{N-1}​l(x_i^{(m,k)}​,u_i^{(m,k)}​) + V_f​(x_N^{(m,k)}​)$
4. Select the set of elite samples $\mathcal{E}^{(k)}$, typically by taking the set of $M_e$ samples with the lowest cost or by using a threshold on the cost
5. Calculate the parameters of a Gaussian distribution to fit the elite samples
   $\mu_{elite}^{(k)} = \frac{1}{M_e} \sum_{U^{(m,k)} \in \mathcal{E}^{(k)}} U^{(m,k)}$
   $\Sigma_{elite}^{(k)} = \frac{1}{M_e} \sum_{U^{(m,k)} \in \mathcal{E}^{(k)}} \Big( U^{(m,k)} - \mu_{elite}^{(k)} \Big) \Big( U^{(m,k)} - \mu_{elite}^{(k)} \Big)^{\top}$
6. Update the control input distribution parameters via smoothing with $\alpha \in (0,1]$
   $\mu^{(k+1)} = \alpha\mu_{elite}^{(k)} + (1-\alpha) \mu^{(k)}$
   $\Sigma^{(k+1)} = \alpha\Sigma_{elite}^{(k)} + (1-\alpha) \Sigma^{(k)}$

Note: if the covariance matrix is diagonal, then it can be updated element-wise with the variance.

After $K_{CEM}$ iterations, the mean of the resulting distribution is taken as the optimal action sequence
$U^* = \mu^{(K_{CEM})}$
and the MPC applies the first element of that sequence then moves to the next step; i.e. receding horizon.

Warm Start

It is common practice to warm start the parameters of the distribution at the next step by the final or "optimal" parameters obtained from the previous step

$$\mu^{(0)}_{next} = \begin{bmatrix} u_0^* \\ u_1^* \\ \vdots \\ u_{N-1}^* \end{bmatrix}$$

Cross Entropy Intuition

CEM can viewed as minimizing the divergence between the sampling distribution $q_{\theta}(U)$ and an ideal distribution concentrated on the elite trajectories.