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.0Create 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
endCreate 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