# Control design for a quadruple-tank system with JuliaSim Control

In this example, we will implement several different controllers of increasing complexity for a nonlinear MIMO process, starting with a PID controller and culminating with a nonlinear MPC controller with state and input constraints.

This example has an associated video where we provide additional context and insights:

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 3 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:

- Linear control using a
**PID controller**. **Linear MPC**with a linear observer and prediction model.- MPC with a linear prediction model but a
**nonlinear observer**. - Linear MPC with
**integral action**and output references. **Nonlinear MPC**using a nonlinear observer and prediction model.

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:

```
using JuliaSimControl
using JuliaSimControl.MPC
using StaticArrays
using Plots, Plots.Measures, LinearAlgebra
using Test
## Nonlinear quadtank
const kc = 0.5
function quadtank(h, u, p = nothing, t = nothing)
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.2, 0.2
ssqrt(x) = √(max(x, zero(x)) + 1e-3) # For numerical robustness at x = 0
xd = SA[
-a1 / A1 * ssqrt(2g * h[1]) + a3 / A1 * ssqrt(2g * h[3]) + γ1 * k1 / A1 * u[1]
-a2 / A2 * ssqrt(2g * h[2]) + a4 / A2 * ssqrt(2g * h[4]) + γ2 * k2 / A2 * u[2]
-a3 / A3 * ssqrt(2g * h[3]) + (1 - γ2) * k2 / A3 * u[2]
-a4 / A4 * ssqrt(2g * h[4]) + (1 - γ1) * k1 / A4 * u[1]
]
end
nu = 2 # number of control inputs
nx = 4 # number of states
ny = 2 # number of measured outputs
Ts = 2 # sample time
w = exp10.(-3:0.1:log10(pi / Ts)) # A frequency-vector for plotting
discrete_dynamics0 = rk4(quadtank, Ts, supersample = 2) # Discretize the nonlinear 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)
```

`FunctionSystem{Discrete{Int64}, JuliaSimControl.MPC.var"#36#37"{Int64, typeof(Main.quadtank), Float64}, Main.var"#2#3", Vector{Symbol}, Vector{Symbol}, Vector{Symbol}, Nothing, UnitRange{Int64}, SciMLBase.NullParameters, UnitRange{Int64}, Nothing}(JuliaSimControl.MPC.var"#36#37"{Int64, typeof(Main.quadtank), Float64}(2, Main.quadtank, 1.0), Main.var"#2#3"(), Discrete{Int64}(2), [:h1, :h2, :h3, :h4], [:u1, :u2], [:h1, :h2], nothing, 1:4, SciMLBase.NullParameters(), 0:-1, nothing)`

Next, we define the **constraints and an operating point**. The maximum allowed control signal will be determied by an interactive slider that is placed further down in the notebook. We start by defining a *desired* state at the operating point, `xr0`

`xr0 = [10, 10, 6, 6]; # desired reference state`

We then solve for the state and control input that is close to the desired state and yields a stationary condition (zero derivative

```
xr, ur = begin # control input at the operating point
using Optim
optres = @views Optim.optimize(
xu ->
sum(abs, quadtank(xu[1:4], xu[5:6], 0, 0)) + 0.0001sum(abs2, xu[1:4] - xr0),
[xr0; 0.25; 0.25],
BFGS(),
Optim.Options(iterations = 100, x_tol = 1e-7),
)
@info optres
optres.minimizer[1:4], optres.minimizer[5:6]
end
```

`([9.820678120303828, 9.82065546734249, 6.285184388729285, 6.285136223039935], [0.26026652506240466, 0.2602685768198778])`

```
# Control limits
umin = fill(0.0, nu)
umax = fill(0.6, nu)
# State limits
xmin = zeros(nx)
xmax = Float64[12, 12, 8, 8]
constraints_pid = MPCConstraints(; umin, umax, xmin, xmax)
x0 = [2, 1, 8, 3] # Initial tank levels
Cc, Dc = JuliaSimControl.linearize(measurement, xr, ur, 0, 0)
op = OperatingPoint(xr, ur, Cc * xr)
```

`OperatingPoint{Vector{Float64}, Vector{Float64}, Vector{Float64}}([9.820678120303828, 9.82065546734249, 6.285184388729285, 6.285136223039935], [0.26026652506240466, 0.2602685768198778], [4.910339060151914, 4.910327733671245])`

## PID control

Our first attempt at controlling the level in the quad-tank system is going use a PID controller. We will tune the controller using the **automatic tuning** capabilities of JuliaSimControl. To make use of the autotuner, we need a **linearized model** of the plant, for which we make use of the function `linearize`

.

```
disc(x) = c2d(ss(x), Ts) # Discretize the linear model using zero-order-hold
Ac, Bc = JuliaSimControl.linearize(quadtank, xr, ur, 0, 0)
Cc, Dc = JuliaSimControl.linearize(measurement, xr, ur, 0, 0)
Gc = ss(Ac, Bc, Cc, Dc)
G = disc(Gc)
```

Since this is a MIMO system and PID controllers are typically SISO, we look for an **input-output pairing** that is approximately **decoupled**. To this end, we investigate the **relative-gain array**(RGA). If we aim for a crossover bandwidth of around 0.01rad/s, we find the optimal input-output pairings for decoupled PID control using the relative gain array:

`relative_gain_array(Gc, 0.01) .|> abs`

```
2×2 Matrix{Float64}:
0.2352 0.851698
0.851698 0.2352
```

Unfortunately, this matrix is rather far from diagonalizable using permutations, indicating that there is a somewhat **strong cross-coupling** in the system and standard PID control is likely to be difficult (the parameter $\gamma$ directly influeces the cross-coupling). Investigating the RGA as a function of frequency, we further notice that for high frequencies, the conclusion about the input-output pairing changes!

```
rgaplot(
Gc,
w,
legend = false,
layout = 4,
plot_title = "RGA plot",
title = "",
ylabel = "",
grid = false,
link = :both,
)
```

In our case, we will stick with the pairing we decided upon for low frequencies, since the input and state constraints will limit how fast we can control the system. We proceed to tune a controller for the $u_1 \rightarrow y_2$ mapping and do this by defining an `AutoTuningProblem`

. This approach lets us put constraints on the largest magnitude of the closed-loop sensitivity functions, $M_S, M_T, M_{KS}$.

```
tuningprob = AutoTuningProblem(;
P = Gc[2, 1],
Ts,
w = exp10.(LinRange(-4, -1, 100)),
Ms = 1.1,
Mt = 1.05,
Mks = 20,
Tf = 1200,
metric = :IAE,
)
```

We solve the problem by calling `solve`

`tuningresult = solve(tuningprob, xtol_rel = 1e-4)`

The autotuning returns a result structure, the PID+filter parameters are available as `tuningresult.p`

while a controller object is available as `tuningresult.K`

. One can further call `OptimizedPID(tuningresult)`

to obtain a ModelingToolkit system that represents the tuned controller including anti-windup etc.

`tuningresult.p`

```
4-element Vector{Float64}:
0.11540232410524603
0.0003779280166623159
9.28124847271885
10.000000779512744
```

```
plot(tuningresult, titlefont = 9)
xlims!((-5, 1), ylims = (-5, 0.5), sp = 4, legend = :bottomleft);
```

The resulting controller respects the sensitivity constraints, visualized in both Bode plots and the Nyquist plot as dashed lines.

We also construct a **static precompensator**`iG0 =`

$G(0)^{-1}$ that decouples the system at DC. This strategy can sometimes mitigate the problem with cross-coupling between the channels, but may be treacherous if the relative-gain array contains large entries (which is fortunately not the case in our problem).

`iG0 = [0 1; 1 0] # No decoupling`

We form the final controller by applying the static precompensator $G_0^{-1}$ to the optimized PID controller

`Cpid = (tuningresult.K * I(2)) * iG0;`

To increase the realism in the simulation of the final system, we add the **saturation nonlinearity** that corresponds to **actuator limitations**, as well as the offsets implied by the linearization point to get the correct units on the signals:

```
using ControlSystemsBase: offset, saturation
Gcop = offset(op.y) * Gc * offset(-op.u)
Cpid_sat = saturation(constraints_pid.umin, constraints_pid.umax) * Cpid
```

The simulation of the closed-loop system controlled by the PID controller is shown belo

```
fig1 = plot(
lsim(feedback(Gcop * Cpid_sat), Gc.C * xr, 0:Ts:1500, x0 = [x0 - op.x; zeros(Cpid.nx)]),
layout = 1,
sp = 1,
title = "Outputs",
ylabel = "",
)
hline!(Gc.C * xr, l = :dash, c = 1, legend = false)
plot(
fig1,
plot(
lsim(
feedback(Cpid_sat, Gcop),
Gc.C * xr,
0:Ts:1500,
x0 = [zeros(Cpid.nx); x0 - op.x],
),
layout = 1,
sp = 1,
title = "Control signals",
ylabel = "",
),
size = (1000, 300),
margin = 4mm,
)
```

The result looks alight, but the problems are multiple:

- The input
**saturation**causes the control signal to frequently hit the control limits and we thus effectively**loose feedback**during large transients. - The rather large
**cross-coupling**between the channels is mostly**ignored**by the PID controller. - The non-measured states that were not allowed to go above certain levels
**overshot this constraint**, shown below.

```
res_pid =
lsim(feedback(Gcop * Cpid_sat), Gc.C * xr, 0:Ts:1000, x0 = [x0 - op.x; zeros(Cpid.nx)])
sfig = plot(
res_pid.t,
res_pid.x[1:4, :]' .+ op.x',
label = string.(permutedims(:x^4)),
title = "States and constraints",
layout = 4,
)
plot!(OvershootObjective.(constraints_pid.xmax), sp = (1:4)')
sfig
```

This demonstrates that it can be treacherous to rely on optimization without taking all aspects of the problem into account. Furthermore, In order to successfully control the tank system and respect input and state constraints using a PID controller, we would have to accept a lower performance, e.g., by lowering the constraint $M_{KS}$ in the autotuning problem.

Fortunately, the MPC framework is very capable of taking input and state consstraints into account, something we will explore soon.

### Robustness analysis

The quadtank being a MIMO system means that the classical gain and phase margins are somewhat hard to apply. A **robustness measure** that is more suitable to the MIMO setting that also tells you something about simultaneous perturbations to both gain and phase at the plant input is the *diskmargin* and disk-based gain and phase margins, which we may plot as a function of frequency:

```
plot(diskmargin(Gc, Cpid, 0, w).simultaneous_input);
ylims!((1, 8), sp = 1, yscale = :identity, legend = false);
```

`dm_pid = diskmargin(Cpid * Gc)`

```
Disk margin with:
Margin: 0.8038583470203824
Frequency: 0.009417766006869529 rad/s, 0.0014988840128761062 Hz
Gain margins: [0.42660559305741647, 2.344085535384462]
Phase margin: 43.79326710514102
Delay margin: 81.15902440478052 s
Skew: 0
Worst-case perturbation: missing
```

We see that we have modest gain and phase margins, certainly not too impressive. Keep in mind, that these margins assumes a linear system without, e.g., the input saturation.

We may also visualize the stable region in the plane of simultaneous gain and phase variations:

`plot(dm_pid)`

## Linear MPC

With a MPC controller, we can take the constraints into account explicitly in the optimization problem solved at each sample time.

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 at a fixed set point.

We proceed in a similar fashion to above, making use of the model linearized around the specified operating point $(x_r,u_r,y_r)$. We also construct a Kalman filter for state estimation.

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

```
constraints_lmpc = MPCConstraints(; umin, umax, xmin, xmax)
R1 = 1e-5 * I(nx) # Dynamics covariance
R2 = I(ny) # Measurement covariance
kf_lmpc = let (A, B, C, D) = ssdata(G)
KalmanFilter(A, B, C, D, R1, R2)
end
pm = LinearMPCModel(
G,
kf_lmpc;
constraints = constraints_lmpc,
op,
x0,
strictly_proper = false,
)
```

We also specify some cost matrices for the MPC problem and the prediction horizon $N$. We will later see how to choose these matrices in more principled ways

```
N = 10 # Prediction horizon
Q1 = 1.0 * I(nx)
Q2 = 1.0 * I(nu)
qs = 100
qs2 = 100000
```

Let's simulate the linear MPC:

```
solver = OSQPSolver(
eps_rel = 1e-5,
eps_abs = 1e-4,
max_iter = 5000,
check_termination = 5,
verbose = false,
sqp_iters = 1,
polish = true, # to get high accuracy
)
prob_lin = LQMPCProblem(
pm;
Q1,
Q2 = 0Q2,
Q3 = Q2,
qs,
qs2,
N,
r = xr,
solver,
)
@time hist_lin = MPC.solve(
prob_lin;
x0,
T = 1000 ÷ Ts,
verbose = false,
noise = 0,
dyn_actual = discrete_dynamics,
)
plot(hist_lin, plot_title = "Linear MPC", legend = :bottomright)
```

The controller performs reasonably well and respects the input constraints. We notice that the control-signal trajectory looks qualitatively different now compared to when the PID controller was used, in particular during the time when the state constraints are active for the upper tanks.

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.

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. We would thus likely end up with a stationary error using the controller above. This highlights a problem with naive MPC control (and similarly for standard LQG control), we do not have any integral action in the controller! We will soon see how we can add integral action, but first we explore how we can **make use of a nonlinear observer** (EKF) together with a linear prediction model in order to **improve the constraint satisfaction**.

## Linear MPC with nonlinear observer

The nonlinear observer will make use of an extended Kalman filter (EKF). When we use a nonlinear observer together with a linear prediction model, we must adjust the inputs and outputs of the observer to account for the fact that the prediction model operates in $\Delta$-coordinates. We do this using an `OperatingPointWrapper`

.

```
kf_ekf = let
A, B, C, D = ssdata(G)
KalmanFilter(A, B, C, D, R1, R2)
end
ekf =
ExtendedKalmanFilter(kf_ekf, discrete_dynamics.dynamics, discrete_dynamics.measurement)
```

To highlight the problem of **lacking integral action**, we throw an **input disturbance** into the mix. The disturbance simulates a leakage in the pump after 500s.

```
function disturbance(u, t)
t > 500 / Ts ? [-0.02, 0] : [0.0, 0]
end
pm_ekf = LinearMPCModel(
G,
JuliaSimControl.OperatingPointWrapper(ekf, op);
constraints = constraints_lmpc,
op,
x0,
strictly_proper = false,
)
prob_ekf = LQMPCProblem(
pm_ekf;
Q1,
Q2 = 0Q2,
Q3 = Q2,
qs,
qs2,
N,
r = xr,
solver,
)
@time hist_ekf =
MPC.solve(prob_ekf; x0, T = 1000 ÷ Ts, dyn_actual = discrete_dynamics, disturbance)
plot(hist_ekf, plot_title = "Linear MPC with nonlinear observer", legend = :bottomright)
vline!([500], lab="Disturbance start", l=(:red, :dash))
```