# Model-Predictive Control for the Research Civil Aircraft system

This tutorial will demonstrate how to setup *nonlinear MPC* using a `GenericMPCProblem`

for the RCAM model, a system with 9 states and 5 control inputs. The system is described in detail in The nonlinear Research Civil Aircraft Model (RCAM).

A model of this system is available from the function `JuliaSimControl.ControlDemoSystems.rcam()`

, the interested reader may learn how to build this model from scratch here. The named tuple returned from the `rcam`

contains the `ModelingToolkit.ODESystem`

describing the full system (`rcam.rcam_model`

), as well as pre-simplified system `rcam.iosys`

. We extract the symbolic variables representing the states and inputs from this simplified system using the functions `states`

and `inputs`

for later use.

```
using JuliaSimControl
using JuliaSimControl.MPC
using ModelingToolkit
rcam = JuliaSimControl.ControlDemoSystems.rcam() # Fetch pre-specified model
x_sym = states(rcam.iosys)
u_sym = ModelingToolkit.inputs(rcam.iosys)
```

```
5-element Vector{Any}:
uA(t)
uT(t)
uR(t)
uE_1(t)
uE_2(t)
```

## Linearize the nonlinear model

In order to perform simple linear robustness analysis of our controller, we may linearize the system around the trim point `(rcam.x0, rcam.u0)`

specified in the `rcam`

tuple, learn more about how to obtain this trim point in Trimming. The function `named_ss`

automatically wraps the linearized system in a `NamedStateSpace`

object with the same signal names as the nonlinear system.

`G = named_ss(rcam.rcam_model, u_sym, x_sym; op=merge(rcam.x0, rcam.u0))`

```
NamedStateSpace{Continuous, Float64}
A =
-0.035360192925429516 0.0 0.0611785836145781 0.0 -1.2298134700138668 0.0 0.0 -9.80890266981996 0.0
0.0 -0.18048335013005334 0.0 1.27132 0.0 -84.9905 9.80890266981996 0.0 0.0
-0.22025616997115 0.0 -0.7064428188115621 0.0 82.21569842621334 0.0 0.0 -0.14672564192696624 0.0
0.0 -0.028580482432579282 0.0 -1.3460022033489867 0.0 0.5842437938260427 0.0 0.0 0.0
-0.0010125818619411207 0.0 -0.03364667421289095 0.0 -1.1072605881880084 0.0 0.0 0.0 0.0
0.0 0.007738100183263082 0.0 0.05541413373025527 0.0 -0.5532893073861173 0.0 0.0 0.0
0.0 0.0 0.0 1.0 0.0 0.014958415519649495 0.0 0.0 0.0
0.0 0.0 0.0 0.0 1.0 0.0 0.0 0.0 0.0
0.0 0.0 0.0 0.0 0.0 1.0001118708398868 0.0 0.0 0.0
B =
0.0 0.10943099805088496 0.0 9.81 9.81
0.0 0.0 2.30116292831638 0.0 0.0
0.0 -7.315699619170419 0.0 0.0 0.0
-0.9486086595966131 0.0 0.36403671047110525 0.04074898647246433 -0.04074898647246433
0.0 -2.9192667107634427 0.0 0.3924 0.3924
-0.019863629888650855 0.0 -0.40809265422057917 0.780390903766977 -0.780390903766977
0.0 0.0 0.0 0.0 0.0
0.0 0.0 0.0 0.0 0.0
0.0 0.0 0.0 0.0 0.0
C =
1.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0
0.0 1.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0
0.0 0.0 1.0 0.0 0.0 0.0 0.0 0.0 0.0
0.0 0.0 0.0 1.0 0.0 0.0 0.0 0.0 0.0
0.0 0.0 0.0 0.0 1.0 0.0 0.0 0.0 0.0
0.0 0.0 0.0 0.0 0.0 1.0 0.0 0.0 0.0
0.0 0.0 0.0 0.0 0.0 0.0 1.0 0.0 0.0
0.0 0.0 0.0 0.0 0.0 0.0 0.0 1.0 0.0
0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 1.0
D =
0.0 0.0 0.0 0.0 0.0
0.0 0.0 0.0 0.0 0.0
0.0 0.0 0.0 0.0 0.0
0.0 0.0 0.0 0.0 0.0
0.0 0.0 0.0 0.0 0.0
0.0 0.0 0.0 0.0 0.0
0.0 0.0 0.0 0.0 0.0
0.0 0.0 0.0 0.0 0.0
0.0 0.0 0.0 0.0 0.0
Continuous-time state-space model
With state names: u(t) v(t) w(t) p(t) q(t) r(t) ϕ(t) θ(t) ψ(t)
input names: uA(t) uT(t) uR(t) uE_1(t) uE_2(t)
output names: u(t) v(t) w(t) p(t) q(t) r(t) ϕ(t) θ(t) ψ(t)
```

## Create dynamics functions

The MPC framework wants the dynamics of the nonlinear system in the form of a `FunctionSystem`

. When calling the constructor of `FunctionSystem`

with an `ODESystem`

as input, code generation is automatically performed using `build_controlled_dynamics`

, the code below leads to a `FunctionSystem`

with `u_sym`

as inputs and `x_sym`

as outputs being created, i.e., for the system below

\[\begin{aligned} \dot x &= f(x, u, p, t) \\ y &= h(x, u, p, t) \end{aligned}\]

we assume that $y = x$ for this example. Learn more about how to estimate $y$ from incomplete measurements in, e.g., Control design for a quadruple-tank system with JuliaSim Control.

When constructing the `dynamics = FunctionSystem(rcam.rcam_model, u_sym, x_sym)`

below, we get continuous-time dynamics representing $\dot x = f(x, u, p, t)$, for simulation purposes, we need to construct a discrete-time version as well. To this purpose, we make use of `MPC.rk4`

, but other options are available, documented under `Discretization`

. We discretize with a sample time of $T_s = 0.02s$, which is about 36 times faster than the fastest time constant of the linearized model.

```
dampreport(G) # Print a table of time constants for the linearized model
nu = G.nu # number of control inputs
nx = G.nx # number of states
Ts = 0.02 # sample time
dynamics = FunctionSystem(rcam.rcam_model, u_sym, x_sym)
discrete_dynamics = MPC.rk4(dynamics, Ts) # Discretize the dynamics using RK4
```

```
| Pole | Damping | Frequency | Frequency | Time Constant |
| | Ratio | (rad/sec) | (Hz) | (sec) |
+--------------------+---------------+---------------+---------------+---------------+
| +0 | -1 | 0 | 0 | -Inf |
| -0.109 | 1 | 0.109 | 0.0173 | 9.19 |
| -0.0148 ± 0.135im | 0.109 | 0.136 | 0.0216 | 67.5 |
| -0.292 ± 0.8im | 0.343 | 0.851 | 0.136 | 3.43 |
| -1.39 | 1 | 1.39 | 0.221 | 0.721 |
| -0.91 ± 1.65im | 0.483 | 1.88 | 0.3 | 1.1 |
```

The trim point stored in the `rcam`

tuple is in the form of dictionaries, mapping states to numerical values. The MPC framework is working with generated code and numerical arrays, and we thus map the values stored in the dicts to numerical arrays, the function `ModelingToolkit.varmap_to_vars`

helps us with this.

```
x0 = SVector(ModelingToolkit.varmap_to_vars(rcam.x0, x_sym)...)
u0 = SVector(ModelingToolkit.varmap_to_vars(rcam.u0, u_sym)...)
p0 = SVector(ModelingToolkit.varmap_to_vars(rcam.rcam_constants, Symbol.(dynamics.p))...)
r = copy(x0)
```

```
9-element StaticArraysCore.SVector{9, Float64} with indices SOneTo(9):
84.9905
0.0
1.27132
0.0
0.0
0.0
0.0
0.0149573
0.0
```

## Create an objective

We have now reached a point where it's time to starting thinking more about the MPC problem. We will make use of a quadratic cost function on the form

\[x(N+1)^T Q_N x(N+1) + \sum_{t=1}^N x(t)^T Q_1 x(t) + u(t)^T Q_2 u(t) + \Delta u(t)^T Q_3 \Delta u(t)\]

To make it easier to choose the weights, we construct scaling matrices $D_y, D_u$ that indicate the typical range of the inputs and outputs.

```
Dy = Diagonal([10, 30, 6, 0.6, 0.3, 0.3, 1.2, 0.25, 1.2])
Du = Diagonal([0.1, 0.5, 0.03, 0.3, 0.3])
C2 = Dy \ I(nx) # The output matrix of `G` is identity
const Q1_scaled = C2'Diagonal(@SVector ones(nx))*C2 # state cost matrix
const Q2_scaled = 0.1*inv(Du)'*Diagonal(@SVector ones(nu))/Du # control cost matrix
const Q3_scaled = Q2_scaled
QNsparse, _ = MPC.calc_QN_AB(Q1_scaled, Q2_scaled, Q3_scaled, discrete_dynamics, Vector(r), Vector(u0), p0) # Compute terminal cost
const QN = Matrix(QNsparse)
```

We now define the cost function components required to realize the cost function above, and package them all into an `Objective`

.

```
running_cost = StageCost() do si, p, t
e = si.x - si.r
u = si.u
dot(e, Q1_scaled, e) + dot(u, Q2_scaled, u)
end
difference_cost = DifferenceCost((si,p,t)->SVector{5}(si.u)) do Δu, p, t
dot(Δu, Q3_scaled, Δu)
end
terminal_cost = TerminalCost() do ti, p, t
e = ti.x - ti.r
dot(e, QN, e)
end
objective = Objective(running_cost, terminal_cost, difference_cost)
```

## Create objective input

Next up, we define an instance of the type `ObjectiveInput`

. This structure allow the user to pass in an initial guess for the optimal solution from the starting state. To provide such a trajectory, we simulate the system forward in time from the initial condition `x0`

using the function `MPC.rollout`

, here, we make use of the discretized dynamics. We also define the MPC horizon length `N = 10`

. The horizon length is a tuning parameter for the MPC controller, and a good initial guess is to match the time constant of the dominant dynamics in the process to be controlled, i.e., if the time constant is $\tau$, $N = \tau / T_s$ is a reasonable start. For this system, we choose `N`

much lower based on empirical tuning.

```
N = 10 # MPC optimization horizon
x = zeros(nx, N+1)
u = zeros(nu, N)
x, u = MPC.rollout(discrete_dynamics, x0, u, p0, 0)
oi = ObjectiveInput(x, u, r)
```

## Create constraints

Similar to how we created objective components above, we now create constraints we want our MPC controller to respect. We will only constrain the control inputs in this example, but we may in general constrain any arbitrary function of the inputs and states. While we're at it, we define typical bounds for the states as well so that we may set the limits of our plots later.

```
umin = [
deg2rad(-25)
deg2rad(-25)
deg2rad(-30)
deg2rad(0.5)
deg2rad(0.5)
]
umax = [
deg2rad(25)
deg2rad(10)
deg2rad(30)
deg2rad(10)
deg2rad(10)
]
xmin = [75, -30, -2, -1, -0.1, -0.2, -0.3, -0.1, -0.1]
xmax = [95, 20, 10, 1, 0.5, 0.5, 1, 0.5, 0.5]
bounds_constraints = StageConstraint(umin, umax) do si, p, t
si.u # The constrained output is just u in this case
end
```

## Create observer solver and problem

The MPC framework requires the specification of an observer that is responsible for feeding (an estimate of) the state of the system into the optimization-part of the MPC controller. In this example, we use the `StateFeedback`

, which is so named due to it not relying on measurements, rather, it knows the state of the system from the simulation model.

We also define the solver we want to use to solve the optimization problems. We will make use of IPOPT in this example, a good general-purpose solver.

```
observer = StateFeedback(discrete_dynamics, Vector(x0))
solver = MPC.IpoptSolver(;
verbose = false,
tol = 1e-4,
acceptable_tol = 1e-2,
max_iter = 100,
constr_viol_tol = 1e-4,
acceptable_constr_viol_tol = 1e-2,
acceptable_iter = 5,
mu_init = 1e-12,
)
```

Next up, we define the transcription scheme we want to use when transcribing the infinite-dimensional continuous-time problem to a finite-dimensional discrete-time problem. In this example, we will use `Trapezoidal`

integration, a method suitable for fast, low-order solving. We provide `scale_x=diag(Dy)`

when creating the problem so that the solver will scale the dynamics constraints using the typical magnitude of the state components. This may improve the numerical performance in situations where different state components have drastically different magnitudes.

`disc = MPC.Trapezoidal(dynamics)`

We are now ready to create the `GenericMPCProblem`

structure.

```
prob = GenericMPCProblem(
dynamics;
disc,
Ts,
N,
observer,
objective,
constraints = [bounds_constraints],
p = p0,
objective_input = oi,
solver,
xr = r,
presolve = true,
jacobian_method = :forwarddiff,
scale_x = diag(Dy),
scale_u = diag(Du),
verbose = false, # Turn on to get diagnostics printing
);
```

## Run MPC controller

With the problem in hand, we may simulate the MPC controller using the function `MPC.solve`

. We choose a random initial condition `x0sim`

to start the simulation from, and provide the discretized dynamics `dyn_actual=discrete_dynamics`

as our simulation model (may be different from the prediction model in the MPC controller, which in this case is using the continuous-time dynamics).

```
x0sim = xmin .+ (xmax .- xmin) .* rand(nx) # A random initial state
history = MPC.solve(prob; x0=x0sim, T = 2000, verbose = false, dyn_actual=discrete_dynamics);
```

The history object that is returned from `MPC.solve`

contains some signals of interest for us to plot:

```
X,E,R,U,Y = reduce(hcat, history)
timevec = range(0, step=Ts, length=size(X,2))
figx = plot(timevec, X', label=permutedims(x_sym), xlabel="Time [s]", layout=9, ylims=(xmin', xmax'), tickfontsize=6, labelfontsize=6)
hline!(r', primary=false, l=(:dash, :black))
figu = plot(timevec[1:end-1], U', label=permutedims(u_sym), layout=5, tickfontsize=6, labelfontsize=6)
hline!([umin umax]', primary=false, l=(:dash, :black))
plot(figx, figu, size=(1000, 1000), tickfontsize=6, labelfontsize=6)
```

If everything went well, we expect to see an initial transient where the MPC controller is navigating the aircraft from the random initial condition to the vicinity of the set-point. This example did not add any integral action to the controller, so we do expect a minor stationary error in some of the signals. Learn more about adding integral action in one of the tutorials