# Constrained nonlinear MPC for MIMO systems

In this example, we will implement an MPC controller with state and input constraints for a nonlinear MIMO process.

The process we will consider is a quadruple tank, where two upper tanks feed into two lower tanks, depicted in the schematics below. The quad-tank process is a well-studied example in many multivariable control courses, this particular instance of the process is borrowed from the Lund University introductory course on automatic control. The process has a cross coupling between the tanks, governed by a parameters $\gamma_i$: The flows from the pumps are divided according to the two parameters $γ_1 , γ_2 ∈ [0, 1]$. The flow to tank 1 is $γ_1 k_1u_1$ and the flow to tank 4 is $(1 − γ_1 )k_1u_1$. Tanks 2 and behave symmetrically.

The dynamics are given by

\begin{aligned} \dot{h}_1 &= \dfrac{-a_1}{A_1 \sqrt{2g h_1}} + \dfrac{a_3}{A_1 \sqrt{2g h_3}} + \dfrac{γ_1 k_1}{A_1 u_1} \\ \dot{h}_2 &= \dfrac{-a_2}{A_2 \sqrt{2g h_2}} + \dfrac{a_4}{A_2 \sqrt{2g h_4}} + \dfrac{γ_2 k_2}{A_2 u_2} \\ \dot{h}_3 &= \dfrac{-a_3}{A_3 \sqrt{2g h_3}} + \dfrac{(1-γ_2) k_2}{A_3 u_2} \\ \dot{h}_4 &= \dfrac{-a_4}{A_4 \sqrt{2g h_4}} + \dfrac{(1-γ_1) k_1}{A_4 u_1} \end{aligned}

where $h_i$ are the tank levels and $a_i, A_i$ are the cross-sectional areas of outlets and tanks respectively. For this system, if $0 \leq \gamma_1 + \gamma_2 < 1$ the system is non-minimum phase, i.e., it has a zero in the right half plane.

In the examples below, we assume that we can only measure the levels of the two lower tanks, and need to use a state observer to estimate the levels of the upper tanks.

The interested reader can find more details on the quadruple-tank process from the manual provided here (see "lab 2"), from where the example is taken.

We will consider several variants of the MPC controller:

1. Nonlinear MPC using a nonlinear observer and prediction model.
2. Linear MPC with a linear observer and prediction model.
3. MPC with a linear prediction model but a nonlinear observer.
4. Linear MPC with integral action and output references.

In all cases we will consider constraints on the control authority of the pumps as well as the levels of the tanks.

We start by defining the dynamics. The tank process has slow dynamics, a typical set-point change may take on the order of 100s.

using JuliaSimControls
using JuliaSimControls.MPC
using Plots, LinearAlgebra
const kc = 0.5
k1, k2, g = 1.6, 1.6, 9.81
A1 = A3 = A2 = A4 = 4.9
a1, a3, a2, a4= 0.03, 0.03, 0.03, 0.03
γ1, γ2 = 0.3, 0.3

ssqrt(x) = √(max(x, zero(x)) + 1e-3) # For numerical robustness at x = 0
xd = [
-a1/A1 * ssqrt(2g*h) + a3/A1*ssqrt(2g*h) +     γ1*k1/A1 * u
-a2/A2 * ssqrt(2g*h) + a4/A2*ssqrt(2g*h) +     γ2*k2/A2 * u
-a3/A3*ssqrt(2g*h)                          + (1-γ2)*k2/A3 * u
-a4/A4*ssqrt(2g*h)                          + (1-γ1)*k1/A4 * u
]
end

nu = 2 # number of controls
nx = 4 # number of states
ny = 2 # number of outputs
Ts = 1 # sample time

discrete_dynamics0 = rk4(quadtank, Ts) # Discretize the continuous-time dynamics
state_names = :h^4
measurement = (x,u,p,t) -> kc*x[1:2]
discrete_dynamics = FunctionSystem(discrete_dynamics0, measurement, Ts, x=state_names, u=:u^2, y=:h^2)

Next, we define the constraints and an operating point

# Control limits
umin = 0 * ones(nu)
umax = 1 * ones(nu)

# State limits (state constraints are soft by default)
xmin = 0 * ones(nx)
xmax = Float64[12, 12, 8, 8]
constraints = MPCConstraints(discrete_dynamics; umin, umax, xmin, xmax)

x0 = [2, 1, 8, 3]       # Initial state
xr = [10, 10, 4.9, 4.9] # reference state
ur = [0.26, 0.26]

The next step is to define an observer. The nonlinear MPC controller will make use of an extended Kalman filter (EKF). While we're at it, we create also a regular Kalman filter for later use with the linear MPC. The regular Kalman filter is also used to construct the EKF, since the EKF wants to know about the covariance information.

R1 = 1e-5*I(nx) # Dynamics covariance
R2 = I(ny)      # Measurement covariance

kf = let
A,B = JuliaSimControls.linearize(discrete_dynamics, xr, ur, 0, 0)
C,D = JuliaSimControls.linearize(measurement, xr, ur, 0, 0)
KalmanFilter(A, B, C, D, R1, R2)
end
ekf = ExtendedKalmanFilter(
kf,
discrete_dynamics.dynamics,
discrete_dynamics.measurement,
)

Next up, we define the MPC problem along with the cost matrices $Q_1, Q_2$, the prediction horizon $N$, and simulate it

solver = OSQPSolver(
eps_rel = 1e-6,
eps_abs = 1e-6,
max_iter = 50000,
check_termination = 5,
sqp_iters = 1,
dynamics_interval = 1,
verbose = false,
polish = false,
)

N = 50
Q1 = 10I(nx)
Q2 = I(nu)
qs = 100
qs2 = 100000

prob = LQMPCProblem(discrete_dynamics; observer=ekf, Q1, Q2, qs, qs2, constraints, N, xr, ur, solver)

hist = MPC.solve(prob; x0, T = 2000, verbose = false, noise=1)
plot(hist, plot_title="Nonlinear MPC")

Let's test that the state converged to our reference and that the constraints were met.

using Test
@test hist.X[end] ≈ xr rtol=1e-1

U = reduce(hcat, hist.U)
@test all(maximum(U, dims=2) .< umax .+ 1e-3)
@test all(minimum(U, dims=2) .> umin .- 1e-3)
Test Passed
Expression: all(minimum(U, dims = 2) .> umin .- 0.001)

As we can see, the nonlinear MPC controller performs quite well and respects the state constraints that the two upper tanks ($h_3, h_4$) are not allowed to reach above a hight of 8.

## Linear MPC

In some situations, we may want to resort to a linear MPC controller. A linear controller is often sufficient when the task is regulation, i.e., keeping a controlled variable aat a fixed set point.

We proceed in a similar fashion to above, defining a linear model that corresponds to the linearization around the specified operating point (xr,ur,yr). (Note, we could make use of the function linearize to perform the linearization numerically, but we show the analytic linearization below to expose its structure)

k1, k2, g = 1.6, 1.6, 9.81
A1 = A3 = A2 = A4 = 4.9
a1, a3, a2, a4= 0.03, 0.03, 0.03, 0.03
h01, h02, h03, h04 = xr
T1, T2 = (A1/a1)sqrt(2*h01/g), (A2/a2)sqrt(2*h02/g)
T3, T4 = (A3/a3)sqrt(2*h03/g), (A4/a4)sqrt(2*h04/g)
c1, c2 = (T1*k1*kc/A1), (T2*k2*kc/A2)
γ1, γ2 = 0.3, 0.3

# Define the process dynamics
Ac = [-1/T1     0 A3/(A1*T3)          0
0     -1/T2          0 A4/(A2*T4)
0         0      -1/T3          0
0         0          0      -1/T4]
Bc = [γ1*k1/A1     0
0                γ2*k2/A2
0                (1-γ2)k2/A3
(1-γ1)k1/A4 0              ]

Cc = kc*[I(2) 0*I(2)] # Measure the first two tank levels
# Cc = kc*I(nx)
Dc = 0
Gc = ss(Ac,Bc,Cc,Dc)

disc(x) = c2d(ss(x), Ts) # Discretize the linear model using zero-order-hold
G = disc(Gc)

kf = let (A,B,C,D) = ssdata(G)
KalmanFilter(A, B, C, D, R1, R2)
end

For a linear MPC controller to work well, we must provide the operating point around which we have linearized. We also construct a LinearMPCModel that keeps track of the model and it's operating point

op = OperatingPoint(xr, ur, Cc*xr)
constraints = MPCConstraints(G; umin, umax, xmin, xmax, op)
pm = LinearMPCModel(G, kf; constraints, op, x0, state_reference=true, strictly_proper=false)

Let's simulate the linear MPC:

prob_lin = LQMPCProblem(pm; Q1, Q2, qs, qs2, constraints,N,xr,solver)

hist_lin = MPC.solve(prob_lin; x0, T = 2000, verbose = false, noise=0, dyn_actual=discrete_dynamics)
plot(hist_lin, plot_title="Linear MPC", legend=:bottomright )

With a linear observer, we notice a slight violation of the state constraints for states $h_3,$h_4\$, remember, we do not measure these states directly, rather we rely on the observer to estimate them. Due to the square root in the dynamics that govern the outflow of liquid from the tanks, the observer thinks that the outflow is greater than it actually is at levels well above the linearization point.

The linear controller also appears to have a slight static error, despite the fact that we supplied an operating point that should pave the way for offset-free regulation. The problem here is that we did not use a sufficient number of significant digits when we specified ur, indeed,

quadtank(xr,ur)
4-element Vector{Float64}:
-0.00025792048495285047
-0.00025792048495285047
-0.0006023452208456792
-0.0006023452208456792

is not exactly zero, while

quadtank(xr,1.01ur)
4-element Vector{Float64}:
-3.226607401828757e-6
-3.226607401828757e-6
-8.059506559965346e-6
-8.059506559965346e-6

is closer. In practice, it's unrealistic to assume that we know the static gain of the system perfectly, in fact, the static gain for this system probably varies with the temperature of the equipment, the tank contents and during the lifetime of the tanks and pumps. This highlights a problem with naive MPC control, we do not have any integral action in the controller! The last example in this tutorial will add integral action, but first we explore how we kan make use of a nonlinear observer (EKF) together with a linear prediction model.

## Linear MPC with nonlinear observer

When we use a nonlinear observer, we do not want to adjust the constraints to the operating point, hence, we recreate the constraints with an empty operating point.

constraints = MPCConstraints(G; umin, umax, xmin, xmax, op=OperatingPoint())
prob_ekf = LQMPCProblem(G; observer=ekf, Q1, Q2, qs, qs2, constraints, N, xr, ur, solver)

@time hist_ekf = MPC.solve(prob_ekf; x0, T = 2000, verbose = false, noise=0.01, dyn_actual=discrete_dynamics)
plot(hist_ekf, plot_title="Linear MPC with nonlinear observer", legend=:bottomright)