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 , where

and the state vector is given by , where:

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

The only input is the elevator angle (rad). The outputs are , , and is the altitude rate (m/s)

The system is subject to the following constraints:

• input constraints ,
• slew rate constraint in the input
• state constraints ,
• output constraints .

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

The controller parameters¶

The book [M02] proposes to use identity matrices of appropriate size for the weighting matrices and . 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.

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 , 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.