Robust MPC tuning using the Glover McFarlane method

In this example, we will show how robust-control tuning methods like the Glover-McFarlane method may be used to tune an MPC controller.

The procedure is

1. Perform traditional loop shaping on the plant $G$ by selecting weights $W_2, W_1$ to obtain the "shaped plant" $G_s = W_2 G W_1$.
2. Find a robustifying controller for the shaped plant using the Glover-McFarlane method.
3. Obtain cost matrices for the MPC controller that makes the MPC controller mimic the Glover-McFarlane controller when no constraints are active (using inverse optimal control).
4. Use the observer found by the Glover-McFarlane method to estimate the state of to initialize the MPC controller each iteration.

The plant we will consider is a slow first order system with additional, much faster, second-order actuator/sensor dynamics

\[G(s) = \dfrac{200}{10s + 1} \dfrac{1}{(0.05s + 1)^2}\]

Preliminary design

The Glover-McFarlane method prompts you to select pre- and post-compensators $W_1, W_2$ that shape the loop-transfer function such that $G_s = W_2 G W_1$ forms the shaped loop-transfer function. See glover_mcfarlane and the references therein for additional details.

We start by designing such a robust controller. We select $W_1$ as a PI-controller that increases the loop gain for low frequencies, and leave $W_2$ unspecified.

using JuliaSimControl, JuliaSimControl.MPC, LinearAlgebra, Plots
using ControlSystems: balance_statespace
default(size=(800, 600))
Ts = 0.01 # Sample time
disc = G -> c2d(balance_statespace(ss(G))[1], Ts)  # A function that discretizes a continuous-time model
G  = tf(200, [10, 1])*tf(1, [0.05, 1])^2 |> disc
W1 = tf([1, 2], [1, 1e-2])               |> disc # A PI controller (with numerically stabilized integrator, hence the 1e-2 factor)
gmf = K, γ, info = glover_mcfarlane(G, 1.1; W1) # Robustify the controller
γ

2.647886182335581

The controller achieves a performance level $\gamma$ (lower is better), typically, we want $\gamma$ less than 4 for a robust design. (Technically, the controller achieves the performance level inv(ncfmargin(G, K)[1]), which may be slightly different than $\gamma$).

We can also check the diskmargin, for both the loop shaped controller and the robustified controller

w = exp10.(LinRange(-2, 2, 200))
plot(diskmargin(G*W1, 0, w), lab="Loop shaping", c=1)
plot!(diskmargin(G*K, 0, w), lab="Robust", c=2)
ylims!((0, 5), subplot=1, yscale=:linear)

This figure shows disk-based upper and lower gain margins as well as phase margin. The gain margin is slightly above two for the robust controller while it's dangerously close to 1 for the non-robustified controller. For an introduction to diskmargins, see "An Introduction to Disk Margins", Seiler et al..

The next step is to use the result of glover_mcfarlane to automatically tune the MPC controller. To this purpose, we use the function inverse_lqr with an argument of type GMF (Glover McFarlane)

sys = info.Gs # Extract the "shaped" plant G*W1
method = GMF(gmf)
Q1,Q2 = inverse_lqr(method)
Q2 = Symmetric(Matrix(1.0*Q2))
Q1

4×4 LinearAlgebra.Symmetric{Float64, Matrix{Float64}}:
 1646.28   679.916   213.537   177.672
  679.916  291.617    91.5863   76.2037
  213.537   91.5863   28.7639   23.9328
  177.672   76.2037   23.9328   19.9131

using this method, $Q_2$ will always be equal to the identity matrix, but $Q_1$ may not be something you would have found by hand tuning.

This gives us the cost matrices for the MPC controller. We also need to construct an observer that uses the observer gains stored in gmf.

x0 = [1.0,2,3,0] # Initial state
H  = -info.Hkf # We need negative feedback
kf = JuliaSimControl.FixedGainObserver(sys, x0, H)

FixedGainObserver{StateSpace{Discrete{Float64}, Float64}, Vector{Float64}, Matrix{Float64}}(StateSpace{Discrete{Float64}, Float64}
A = 
  0.9999939632555839       0.07951210789584315  0.005605459059841839  0.0003075359075296777
 -0.00021896324452507185   0.9823017330979585   0.1309268522542291    0.011154863529085264
 -0.002557165083090411    -0.20672842033596772  0.6541663096357968    0.13027221799295793
  0.0                      0.0                  0.0                   0.9999000049998333
B = 
 0.0001545406570500893
 0.005605459059841842
 0.06546342612711455
 0.00999950001666625
C = 
 7.812499999999998  0.0  0.0  0.0
D = 
 0.0

Sample Time: 0.01 (seconds)
Discrete-time state-space model, [1.0, 2.0, 3.0, 0.0], [0.018729935285477547; 0.015333315396526333; -0.004611731588541434; 0.009279918211332023;;])

Next, we may specify and solve the MPC problem as usual

nx = sys.nx
nu = sys.nu
N  = 10 # Prediction horizon
r = zeros(nx) # reference state

# Control limits
umin = -20 * ones(nu)
umax =  20 * ones(nu)

# State limits (state constraints are soft by default)
xmin   = -1000 * ones(nx)
xmax   =  1000 * ones(nx)
constraints = MPCConstraints(; umin,umax,xmin,xmax)
solver = OSQPSolver()
T      = 150 # Simulation horizon
model = LinearMPCModel(sys, kf; constraints, x0)
prob   = LQMPCProblem(model; Q1,Q2,N,r,solver)
hist   = MPC.solve(prob; x0, T, noise=0.1)
f1 = plot(hist)

The state trajectories shown in the plot represent the state of the shaped plant. In order to simulate how the controller would have performed on the original plant, we may extract the control signal and perform a simulation on the original plant directly:

u = reduce(hcat, hist.U) # Control signal
plot(lsim(sys, u; x0), plotu=true)

Above, we performed a simulation under ideal circumstances, where the prediction model used by the MPC controller was identical to the actual system dynamics. To verify the robustness of the controller, we can simulate a scenario in which the prediction model differs from the actual dynamics (as is usually the case in practice).

The function solve takes a keyword argument dyn_actual where the true dynamics can be supplied, if it's left out, it defaults to use the same as the prediction model. For simulation purposes, we construct a slightly different plant where the gain is off by 2x (recall the gain margin, this is getting close) and the main time constant is slower than modeled, but the sensor dynamics is faster than modeled:

Gact  = tf(2*200, [12, 1])*tf(1, [0.05, 1])*tf(1, [0.02, 1]) |> disc
bodeplot([G, Gact], lab=["Prediction model" "Prediction model" "Actual dynamics" "Actual dynamics"])

Gact = Gact + ss([0], [0], [0], 0, Gact.timeevol) # Add an empty state to the model corresponding to W1
hist2   = MPC.solve(prob; x0 = x0, T, noise=0.1, reset_observer=true, dyn_actual=Gact)
plot!(f1, hist2, l=:dash, c=[1 2 3 4 1 2])

The response is worse this time around (dashed), but still not too bad. Without the robust tuning, the controller would have likely made the system unstable, remember, the disk-based gain margin of the loop-shaping controller was much smaller than the gain error (2x) we simulated.

The interested reader might recall that we mentioned the nfcmargin above. A theorem from robust control says that the controller $K$ will stabilize all plants $\tilde{G}$ that differ no more than nfcmargin(G, K) from $G$ in the $\nu$-gap metric. Let's verify that this holds

nugap(G, Gact)

(0.400913803662315, 19.55660478552275)

ncfmargin(G, K)[1]

0.3862960435573536

in this case, the actual plant $G_{act}$ is in fact too different from $G$ for this theorem to prove stability of the system, fortunately, the condition of the theorem is only sufficient but not necessary, and indeed, we have

ncfmargin(Gact, K)[1]

0.20180334942984213

Linear system verification

Below, we show how to perform the robustness verification without using the MPC controller, we compare the loop-shaping controller $W_1$, without the additional robustification of Glover McFarlane, to the final controller.

The closed-loop transfer function from a load disturbance to the output is given by

\[PS(s) = \dfrac{G_d(s)}{I + G(s)K(s)} = G_d(s)S(s)\]

where $G_d$ is the disturbance model and $S$ is the sensitivity function.

Gd = tf(100, [12, 1]) |> disc # Model from load disturbance to output
PSloop = Gd*output_sensitivity(Gact, W1) # Gd / (I + G*W1) is the transfer function from an input disturbance to output
PSrobust = Gd*output_sensitivity(Gact, K)
plot(step(PSloop), lab="loop shaping K", plot_title="Step responses from load disturbance")
plot!(step(PSrobust), lab="robust K")

Also this test indicates that the robustified controller performs much better than the PI controller $W_1$.