A more complex MPC problem

In this section we consider a more elaborated example. However, the procedure to follow is the same: describe the problem, generate C-code from it, and finally use the generated code.

The MPC problem Description

We now consider a problem that presents many of the features available in the ltidt module. The code for this example can be found inside the tutorial directory muaompc_root/muaompc/_ltidt/tutorial, where muaompc_root is the path to the root directory of muaompc.

The system description

The system considered is the Cessna Citation 500 aircraft presented in ([M02], p.64). A continuous-time linear model is given by \dot{x} = A_c x + B_c u, y = C x, where

A_c = \left[ \begin{matrix}
-1.2822 & 0 & 0.98 & 0 \\
0 & 0 & 1 & 0 \\
-5.4293 & 0 & -1.8366 & 0 \\
-128.2 & 128.2 & 0 & 0 \\
\end{matrix} \right], \;\;
B_c = \left[ \begin{matrix}
-0.3 \\
0 \\
-17 \\
0 \\
\end{matrix} \right],
C = \left[ \begin{matrix}
0 & 1 & 0 & 0 \\
0 & 0 & 0 & 1 \\
-128.2 & 128.2 & 0 & 0 \\
\end{matrix} \right],

and the state vector is given by x = [x_1 \; x_2 \; x_3 \; x_4]^T, where:

  • x_1 is the angle of attack (rad),
  • x_2 is the pitch angle (rad),
  • x_3 is the pitch angle rate (rad/s), and
  • x_4 is the altitude (m).

The only input u_1 is the elevator angle (rad). The outputs are y_1 = x_2, y_2 = x_4, and y_3 = -128.2 x_1 + 128.2 x_2 is the altitude rate (m/s)

The system is subject to the following constraints:

  • input constraints -0.262 \leq u_1 \leq 0.262,
  • slew rate constraint in the input -0.524 \leq \dot{u}_1 \leq 0.524
  • state constraints -0.349 \leq x_2 \leq 0.349,
  • output constraints -30.0 \leq y3 \leq 30.0.

To consider the slew rate constraint in the input, we introduce an additional state x_5. The sampling interval is dt = 0.5 s, and the horizon length is N = 10 steps.

The controller parameters

The book [M02] proposes to use identity matrices of appropriate size for the weighting matrices Q and R. We instead select them diagonal with values that give a similar controller performance and much lower condition number of the Hessian of the MPC quadratic program (see Conditioning the Hessian), a desirable property for any numerical algorithm.

The system module

In this case, our system module is called sys_aircraft.py. The system matrices Ac and Bc have been already discretized, because the slew rate constraint is easier to include in this way. It looks as follows:

from numpy import diag  # similar effect with: from numpy import *

dt = 0.5
N = 10
mu = 100
# discrete-time system
Ad = [[  0.23996015,   0., 0.17871287,   0., 0.],
      [ -0.37221757,   1., 0.27026411,   0., 0.],
      [ -0.99008755,   0., 0.13885973,   0., 0.],
      [-48.93540655, 64.1, 2.39923411,   1., 0.],
      [0., 0., 0., 0., 0.]]
Bd = [[-1.2346445 ],
      [-1.43828223],
      [-4.48282454],
      [-1.79989043],
      [1.]]
# Weighting matrices for a problem with a better condition number
Q = diag([1014.7, 3.2407, 5674.8, 0.3695, 471.75])
R = diag([471.65])
P = Q
# input constraints
eui = 0.262  # rad (15 degrees). Elevator angle.
u_lb = [[-eui]]
u_ub =  [[eui]]
# mixed constraints
ex2 = 0.349  # rad/s (20 degrees). Pitch angle constraint.
ex5 = 0.524 * dt  # rad/s * dt input slew rate constraint in discrete time
ey3 = 30.
# bounds
e_lb = [[-ex2], [-ey3], [-ex5]]
e_ub = [[ex2], [ey3], [ex5]]
# constraint matrices
Kx = [[0, 1, 0, 0, 0],
      [-128.2, 128.2, 0, 0, 0],
      [0., 0., 0., 0., -1.]]
Ku = [[0],
      [0],
      [1]]
# terminal state constraints
f_lb = e_lb
f_ub = e_ub
F = Kx

Before we continue, let us make a few remarks. We use numpy to help us build the diagonal matrices Q and R, using the function diag. Finally, compare the name of the variables used in the system module against the MPC problem described in Section The ltidt module. Additionally, the optional penalty parameter mu has been selected using the procedure described in Section Basics of tuning. Finally, the weighting matrices Q and R where transformed from identity matrices to the ones shown above using the functions presented in Section Conditioning the Hessian.

Generating the C-code

Now that we have written the system module, we proceed to create an instance of a muaompc.ltidt class. Change to the directory containing the file sys_aircraft.py and in a Python interpreter type:

import muaompc

mpc = muaompc.ltidt.setup_mpc_problem('sys_aircraft')
mpc.generate_c_files()

If everything went okay, you should now see a new directory called cmpc. Alternatively, switch to the tutorial directory and execute the file called main_aircraft.py, which contains the same Python code as above.

Using the generated C-code

In the cmpc directory you will find the automatically generated code for the current example (if you previously generated code for the basic tutorial, it will be overwritten). Included is also an example Makefile, which compiles the generated code into a library using the GNU Compiler Collection (gcc).

The next step is to make use of the generated code. The first part of the main_aircraft.c file found in the tutorial directory is identical to the first part of the main_motor.c file found in A basic MPC problem.

Algorithm configuration

The next step is to configure the algorithm. In this case, we have a system with input and state constraints. The only parameters to configure are the number of iterations of the algorithm. The state constrained algorithm is an augmented Lagrangian method, which means it requires a double iteration loop (an internal and an external loop). From simulation we determine that 24 internal iterations, and 2 external iterations provide an acceptable approximation of the MPC problem using the warmstart strategy:

ctl.conf.in_iter = 24; /* number of internal iterations */
ctl.conf.ex_iter = 2; /* number of external iterations */
ctl.conf.warmstart = 1; /* automatically warmstart algorithm */

Closed loop simulation

We can finally simulate our system. We start at some state x = (0, 0, 0, -400, 0)^T, and the controller should bring the system to the origin. In this case, we simulate for s=40 steps. We solve the problem for the current state by calling mpc_ctl_solve_problem(&ctl, x);. The solution is stored in an array of size MPC_HOR_INPUTS (the number of inputs times the horizon length) pointed by ctl.u_opt. We can get access to its first element using array notation, e.g. ctl.u_opt[0]. The function mpc_predict_next_state replaces the current state with the successor state. The complete example code looks like:

#include <stdio.h>  /* printf */
#include "cmpc/include/mpc.h"  /* the auto-generated code */

/* This file is a test of the C routines of the ALM+FGM MPC
 * algorithm. The same routines can be used in real systems.
 */
int main(void)
{
  real_t x[MPC_STATES];  /* current state of the system */
  extern struct mpc_ctl ctl;  /* already defined */
  int i;  /* loop iterator */
  int j;  /* print state iterator */
  int s = 40;  /* number of simulation steps */

  ctl.conf->in_iter = 24;  /* iterations internal loop */
  ctl.conf->ex_iter = 2;  /* iterations external loop */
  ctl.conf->warmstart = 1;  /* warmstart each iteration */

  /* The current state */
  x[0] = 0.;
  x[1] = 0.;
  x[2] = 0.;
  x[3] = -400.;
  x[4] = 0.;

  for (i = 0; i < s; i++) {
    /* Solve and simulate MPC problem */
    mpc_ctl_solve_problem(&ctl, x);   /* solve the MPC problem */
    mpc_predict_next_state(&ctl, x);
    /* print first element of input sequence and predicted state */
    printf("\n step: %d  - ", i);
    printf("u[0] = %f; ", ctl.u_opt[0]);
    for (j = 0; j < MPC_STATES; j++) {
      printf("x[%d] = %f; ", j, x[j]);
    }
  }
  printf("\n SUCCESS! \n");

  return 0;
}

Running the code

In the current directory, you will find among others, the file main_aircraft.c which contains the code above, together with a Makefile. Compile the code by typing make aircraft (you might need to edit your Makefile). If compilation was successful, you should see a new executable file called aircraft. If you run it, the word SUCCESS! should be visible at the end of the text displayed in the console.

Testing with Python

Let’s try doing the same using the Python interface. As usual, go to the to the tutorial directory, launch your Python interpreter, and in it type:

import muaompc
import numpy as np
mpc = muaompc.ltidt.setup_mpc_problem("sys_aircraft")
ctl = mpc.ctl
s = 40
ctl.conf.in_iter = 24
ctl.conf.ex_iter = 2
ctl.conf.warmstart = True
n, m = mpc.size.states, mpc.size.inputs
x = np.zeros([n, 1])
x[3] = -400
for i in range(s):
    ctl.solve_problem(x)
    x = mpc.sim.predict_next_state(x, ctl.u_opt[:m])
    # x = ctl.sys.Ad.dot(x) + ctl.sys.Bd.dot(ctl.u_opt[:m])  # predict
    print("step:", i, "- u[0] = ", ctl.u_opt[0], "; x = ", x.T)
print("SUCCESS!")

Compared to the C code above, there are a few things to note. A function similar to mpc_predict_next_state is available in Python under the object sim (see A simulation class in Python). This is a convenience function for C and Python that computes exactly what the line marked with # predict does. Note that in Python and in C the structure ctl.sys (the system) and many other data structures are available. The MPC object (not available in C) offers in Python additional data structures. In this example we used mpc.size, which contains the size of all relevant vectors and matrices. Also note that ctl.sys.Ad and x are NumPy arrays, therefore the need to use the dot method.

Where to go next?

Several examples are included in the folder muaompc_root/examples. Detailed explanation of \muaompc functionality are presented in the following chapters.