Lab 8B: Closed Loop Control¶

1 Model Predictive Control (MPC)¶

In Lab 8A we tried to control a dynamical system with model based optimization. This strategy failed because of the mismatch between the model and the true system.

In model based optimization we find control actions $\hat{u}(t)$ that are optimal for the model of the true dynamical system. This is represented by the bottom part of the diagram where we show that the controller is optimizing with respect to model trajectories $\hat{x}(t)$. Once we find actions that are optimal for the model, we turn around and implement them in the true model. This is shown in the top part of the diagram where the control input of the true dynamical system is $u(t) = \hat{u}^*(t)$.

If we initialize the model and the true system with the same initial conditions, the trajectory $\hat{x}(t)$ of the model and the trajectory $x(t)$ of the true system are initially close. The first few control actions $\hat{u}^*(t)$ are therefore good control actions for the true system. Eventually, however, the trajectory $\hat{x}(t)$ of the model and the trajectory $ x(t)$ of the true system deviate. At this point actions $\hat{u}^*(t)$ that are chosen because they are optimal for the model are far from good for the true system.

This explanation suggests a solution. What we need to do is reinitialize the model every so often. This is called model predictive control (MPC).

In MPC we define a window of length $W$ and solve the optimization problem,

\begin{align} \hat{u}^*(t:t+W) ~=~ & \text{argmin}_{\hat{u}(s)} ~ \frac{1}{W} \sum_{~s=t+1~}^{~t+W~} \ell\, \Big( \, x_{R}(s) , \, \hat{x}(s) \, \Big) , \nonumber \\ & \text{ with } \quad ~ \hat{x}(s+1) = A \hat{x}(s) + B \hat{u}(s), \phantom{\Big(}\nonumber \\ & \phantom{\text{with}} ~ \hat{x}(t)= x(t) .\phantom{\Big(} \end{align}

This is the same model based optimization problem we encountered in Lab 8A, except for two modifications: (i) The initial condition is $\hat{x}(t)= x(t)$. (ii) The merit of the trajectory is evaluated between times $s=t+1$ and $s=t+W$. These are not significant differences. Modification (ii) is just a practical consideration to reduce the complexity of the optimization problem. This is achieved by reducing the time horizon from $T$ to $W$. Modification (i) is just a shift in time. We initialize the model with the state at time $t$ instead of initializing the model with the state at time $0$

The significant difference is that we now extract the control action $ u^*(t)$ and execute this action in the true dynamical system. The remaining actions $ u^*(t+1:t+W)$ that are also part of the solution are discarded. We then proceed to advance time to $t+1$ and solve the model based optimization with initial condition $\hat{x}(t+1)= x(t+1)$.

The MPC controller is what we call a closed loop control system. We observe the state $ x(t)$ at time $t$ and use this information to select the control action $ u(t)=\hat{u}^*(t)$. The effect of this control action is to move the true system into state $ x(t+1)$. The terminology of closing the loop refers to the fact that we now observe the state $ x(t+1)$ and use this observation to select the control action $ u(t+1)=\hat{u}^*(t+1)$. This is different from the open loop control strategy in which we observe the initial condition $ x(0)= x_0$ but never again observe the state of the true system.

0 Environment Setup¶

For this lab, we will need some external code. The next cell will clone a repository with the code we need.

Make sure you have git installed in your system, or use Colab.

Load data and functions from Lab 8A¶

Task 1¶

Leverage the model based optimization code of Lab 8A to run MPC control on the car simulator. Implementing MPC requires that you implement the following steps at all times $t$,

1. Observe the current state $ x(t)$ of the true system.

2. Initialize the model system with $\hat{x}(t)= x(t)$.

3. Solve the MPC problem. The outcome is a sequence $ u^*(t:t+W)$.

4. Implement control action $ u(t)=\hat{u}^*(t)$ in the true system.

Step (S3) is where we compute the control action $ u(t)=\hat{u}^*(t)$. We actually compute a sequence of control actions $ u^*(t:t+W)$ but we discard all entries of the sequence but the first. Steps (S4) implements the control action in the true system. Steps (S1) and (S2) close the control loop by observing the current state of the true system and feeding it as an input to the model system.

Plot the reference trajectory $ x_{R}(t)$ and the trajectory $ x(t)$ generated by implementation of the MPC controller. Slow down the plot to create a movie of the controlled car following the reference trajectory.

Evaluate and plot the loss between the generated trajectory and the reference trajectory.

The trajectory generated by the MPC controller follows the reference trajectory with small loss. This is because closing the loop precludes the accumulation of errors.

It is interesting to think of the model part of the controller as a simulation of the true dynamical system. We use this simulation to evaluate the merit of different control actions as a way of deciding which control action to execute. This is a very human way of thinking. We use models to simulate the effects of different courses of action. We then execute the action that our simulations deem best. Because simulations and reality are in the end different, we observe the outcome of our actions and repeat the simulation to select a new course of action.

2 Imitation Learning¶

The MPC controller has a high computation cost as it requires the solution of an optimization problem in each time step. This is a challenge because the time between time steps can be short. In the car driving scenario we are considering, the sampling time is $T_s=50ms$.

We can avoid this computation cost using learning. One approach is to consider the MPC solutions and train a learning parametrization that maps the initialization $\hat{x}(t)= x(t)$ into the control action $ u(t)=\hat{u}^*(t)$. We do that by introducing a learning parametrization $\pi( x(t);\theta)$ and defining the optimization problem,

\begin{align} \theta^* ~=~ & \text{argmin}_{\theta} ~ \frac{1}{T} \sum_{t=1}^{T} \ell\, \Big( \, \hat{u}^*(t),\, \pi\big( x(t);\theta\big) \, \Big) . \end{align}

In this problem $ x(t)$ is the initialization of the MPC controller and $\hat{u}^*(t)$ is the optimal control action produced as the corresponding solution of the optimization problem.

We say that this is an imitation problem because we are training a policy $\pi( x(t);\theta)$ that imitates the actions of the MPC controller. The advantage of training this imitation policy is that we can train it offline, as we always do. At execution time we just evaluate the policy $\pi( x(t);\theta^*)$ and execute the corresponding control action.

We can consider the observed states $ x_q(t)$ and undertake

\begin{align} \theta^* ~=~ & \text{argmin}_{\theta} ~ \frac{1}{Q} \sum_{q=1}^{Q} ~ \frac{1}{T} \sum_{t=1}^{T} \ell\, \Big( \, \hat{u}_q^*(t),\, \pi\big( x_q(t);\theta\big) \, \Big), \end{align}

in which $\hat{u}_q^*(t)$ is the solution of the MPC control optimization problem when the initial condition is $\hat{x}(t)= x_q(t)$.

A third possibility is to dither the expert trajectories. That is, for each state $ x_q(t)$ we generate $D$ states $\tilde{x}_{qd}(t)$ by adding a random perturbation $ w_{qd}(t)$,

\begin{align} \tilde{x}_{qd}(t) = x_q(t) + w_{qd}(t). \end{align}

We then reformulate by considering all trajectories, all points in time, and all dithered states. This leads to the optimization problem,

\begin{align} \theta^* ~=~ & \text{argmin}_{\theta} ~ \frac{1}{Q} \sum_{q=1}^{Q} ~ \frac{1}{T} \sum_{t=1}^{T} ~ \frac{1}{D} \sum_{d=1}^{D} \ell\, \Big( \, \hat{u}_q^*(t),\, \pi\big(\tilde{x}_{qd}(t); \theta\big) \, \Big), \end{align}

in which $\hat{u}_q^*(t)$ is reinterpreted as the solution of the MPC control optimization problem when the initial condition is $ x(t)=\tilde{x}_{qd}(t)$. Adding dithering to the training set improves the robustness of the learned controller.

The three optimization problems have the same objective but take averages over different datasets. Thus, their relative merits depend on which of the two set of input states is more representative of the states that are to be expected at execution time. The possible mismatch between the states that we choose for optimizing $\theta$ and the states that are actually seen at execution time is a significant challenge when learning to imitate control policies of dynamical systems.

Task 2¶

Learn a policy that imitates the MPC controller. You can choose any parametrization you like, but to keep matters simple we recommend that you use a linear parameterization,

\begin{align} \pi\big( x(t);\theta\big) = \Theta ( x(t)- x_{R}(t)) \end{align}

You can also use any loss you want. If you use a linear parameterization, a quadratic loss is a good idea.Train the imitation policy. Consider $D=1$ dithered state per $ x_{qd}(t)$ and use white Gaussian noise $ w_{qd}(t)$ with mean $\mathbb{E}[ w_{qd}(t)]=0$ and variance $\mathbb{E}[ w^2_{qd}(t)]=0.09$.

Evaluate the control policy $\pi( x(t);\theta^*)$ on the car simulator. Plot the reference trajectory $ x_R(t)$ and the trajectory $ x(t)$ generated by the imitation policy. Evaluate and plot the loss between the generated trajectory and the reference trajectory.

3 Model Predictive Control Learning¶

A second alternative to learn a control policy is direct incorporation of the parameterization from section 1. To do that, we consider a fixed parameter $\theta$ and the policy $\pi(\mathbf{x};\theta)$ that is generated by this parameter. If we execute this policy on the system’s model we incur the cost

\begin{align} \ell_{\text{MPC}}\big(\mathbf{x}(t); \theta\big) = & \frac{1}{W} \sum_{s=t+1}^{t+W} \ell\, \big( \, \mathbf{x}(s) , \, \hat{\mathbf{x}}(s) \, \big) , \\ & \text{with} ~ \hat{\mathbf{x}}(s+1) = \mathbf{A} \hat{\mathbf{x}}(s) + \mathbf{B} \mathbf{u}(s), \\ & \phantom{\text{with}} ~ \hat{\mathbf{x}}(t) = \mathbf{x}(t) , \\ & \phantom{\text{with}} ~ \mathbf{u}(t) = \pi\big(\mathbf{x}(t);\theta\big). \end{align}

As is has been true throughout this chapter, this is not the true cost incurred by the policy $\pi(\mathbf{x};\theta)$ in the actual system. To evaluate this cost we would need to propagate actions through the system function $f(x(t),u(t))$. This is impossible because we do not know this function. The first merit of this loss function is that it can be evaluated. Its second merit is that if model and reality are not that different, a policy that works well in the model works well in the true system.

The loss function defined above is different from loss functions that we have encountered before because it evaluates the merit of the policy $\pi(\mathbf{x};\theta)$ over a trajectory of $W$ time units. A strange loss function it may be, but a loss function nonetheless. We can then use it to propose the learning problem,

\begin{align} \theta^* ~=~ \arg\min_\theta \frac{1}{T} \sum_{t=1}^T ~ \ell_{\text{MPC}}\big(\mathbf{x}(t); \theta\big). \end{align}

As is also the case of Section 2 this does not have to be the case. We can train by minimizing the MPC loss over any set of states $\mathbf{x}(t)$. In particular, if we use expert trajectories to define the optimal parameter $\theta^*$ then we have,

\begin{align} \theta^* ~=~ & \arg\min_{\theta} ~ \frac{1}{Q} \sum_{q=1}^{Q} ~ \frac{1}{T} \sum_{t=1}^{T} \ell_{\text{MPC}}\big(\mathbf{x}_q(t); \theta\big). \end{align}

Observe that this formulation is akin to out imitation from expert trajectories. Except that instead of searching for a parameter $\theta$ that imitates the MPC policy $\hat{\mathbf{y}}_q^*(t)$, we search for a parameter $\theta$ that minimizes the MPC cost

As the reader may be expecting, the use of dithered trajectories is also justified

\begin{align} \theta^* ~=~ & \arg\min_{\theta} ~ \frac{1}{Q} \sum_{q=1}^{Q} ~ \frac{1}{T} \sum_{t=1}^{T} ~ \frac{1}{D} \sum_{d=1}^{D} \ell_{\text{MPC}}\big(\mathbf{x}_{qd}(t); \theta\big). \end{align}

This formulation is akin to the holistic formulation seen earlier in section 2, but we seek to minimize the MPC cost instead of imitating the MPC policy $\hat{\mathbf{u}}_q^*(t)$. As in imitation learning, adding dithering to the training set improves the robustness of the learned controller.

Task 3¶

Learn a policy that minimizes the MPC loss defined. You can choose any parametrization you like, but to keep matters simple we recommend that you use a linear parameterization as in. You can train the imitation policy by solving any of the three learning problems. Whichever you use, explain your reasons for choosing it.

Evaluate the control policy $\pi(\mathbf{x}(t);\theta^*)$ on the car simulator. Plot the reference trajectory $\mathbf{x}_R(t)$ and the trajectory $\mathbf{x}(t)$ generated by the learned policy. Slow down the plot to create a movie of the controlled car following the reference trajectory.

Evaluate and plot the loss between the generated trajectory and the reference trajectory. \hfill$\blacksquare$