Disturbance modeling and rejection with LQG controllers

This example will demonstrate how you can add disturbance models to a plant model and achieve effective disturbance rejection using an LQG controller. For simplicity, we will consider a simple first-order system $G$

\[\begin{aligned} \dot{x} &= -ax + b(u + d) \\ y &= cx \end{aligned}\]

where a load disturbance $d$ is acting on the input of the system. This is a simple and very common model for load disturbances. In this example, we will let $d$ be a unit step at time $t=10$, this will effectively create a LQG controller with integral action.

We will begin by setting up the LQG problem and solve it without any disturbance model. For details regarding the setup of an LQG problem, see, the LQGProblem documentation.

We start by defining the process model and discretize it using zero-order hold.

using RobustAndOptimalControl, ControlSystemsBase, Plots, LinearAlgebra
Ts = 1 # Sample time
G = c2d(ss(tf(1, [10, 1])), Ts) # Process model

StateSpace{Discrete{Int64}, Float64}
A = 
 0.9048374180359594
B = 
 0.23790645491010104
C = 
 0.4
D = 
 0.0

Sample Time: 1 (seconds)
Discrete-time state-space model

We then choose the parameters of the LQG controller, i.e., the cost matrices $Q_1, Q_2$ as well as the covariance matrices $R_1, R_2$

nx  = G.nx
nu  = G.nu
ny  = G.ny
x0  = zeros(G.nx) # Initial condition

Q1 = 100diagm(ones(G.nx)) # state cost matrix
Q2 = 0.01diagm(ones(nu))  # control cost matrix

R1 = 0.001I(nx) # State noise covariance
R2 = I(ny)      # measurement noise covariance
prob = LQGProblem(G, Q1, Q2, R1, R2)

disturbance = (x, t) -> t * Ts ≥ 10 # This is our load disturbance, a step at ``t = 10``
Gcl = G_PS(prob)  # This forms the transfer function from load disturbance to output
res = lsim(Gcl, disturbance, 100)
plot(res)

As we can see, the controller appears to do next to nothing to suppress the disturbance. The problem is that the Kalman filter does not have a model for such a disturbance, and its estimate of the state will thus be severely biased.

The next step is to add a disturbance model to the plant model. Since the disturbance is of low-frequency character (indeed, its transfer function is $1/s$), we make use of the function add_low_frequency_disturbance

Gd = add_low_frequency_disturbance(G)
nx = Gd.nx

The disturbance variable is uncontrollable, so we exclude it from the state-feedback Riccati equation and penalize the physical output only. The output penalty corresponds to the following state-variable penalty:

\[y^T Q_1 y = (Cx)^T Q_1 Cx = x^T (C^T Q_1C) x\]

C  = Gd.C
Q1 = 100diagm(ones(ny)) # output cost matrix
Pe = ExtendedStateSpace(Gd; B1=I, C1=C)
x0 = zeros(nx)

We also provide new covariance matrices for the Kalman filter where the entry of the state-covariance matrix that corresponds to the disturbance variable (the last entry of the augmented state vector) determines how fast the Kalman filter integrates the disturbance. We choose a large value (1), implying fast integration

R1 = diagm([0.001, 1])
R2 = I(ny)
disturbance_indices = (G.nx+1):Gd.nx
disturbance_input_indices = 1:nu
prob = LQGProblem(Pe, Q1, Q2, R1, R2;
    disturbance_indices, disturbance_input_indices)
Gcl  = [G_PS(prob); -comp_sensitivity(prob)] # -comp_sensitivity(prob) is the same as the transfer function from load disturbance to control signal
res  = lsim(Gcl, disturbance, 100)
plot(res, ylabel=["y" "u"]); ylims!((-0.05, 0.3), sp = 1)

disturbance_indices identifies the appended disturbance variables, while disturbance_input_indices pairs them with the control inputs they affect. The Kalman filter estimates the complete augmented state, and lqr(prob) combines the physical feedback gain with disturbance compensation, giving $u=-L_x\hat{x}-\hat{d}$ in this example.

This time, we see that the controller indeed rejects the disturbance and the control signal settles on -1 which is exactly what's required to counteract the load disturbance of +1.

using Test
@test lqr(prob)[:, disturbance_indices] ≈ I(nu)

Test Passed

Before we feel confident about deploying the LQG controller, we investigate its closed-loop properties.

w = exp10.(LinRange(-3, log10(pi / Ts), 200))
gangoffourplot(prob, w, lab = "", legend = :bottomright)

We see that our design led to a system with a rather high peak in sensitivity. This is an indication that we perhaps added too much "integral action" by a too fast observer pole related to the disturbance variable. Let's see how a slightly more conservative design fares:

R1 = diagm([0.001, 0.2]) # Reduce the disturbance-variable noise covariance
R2 = I(ny)
prob = LQGProblem(Pe, Q1, Q2, R1, R2;
    disturbance_indices, disturbance_input_indices)

Gcl = [G_PS(prob); -comp_sensitivity(prob)]
res = lsim(Gcl, disturbance, 100)
f1 = plot(res, ylabel=["y" "u"]); ylims!((-0.05, 0.3), sp = 1)
f2 = gangoffourplot(prob, w, lab = "", legend = :bottomright)

plot(f1, f2, titlefontsize=10)

We see that the smaller disturbance covariance gives a slightly larger disturbance response, but lowers the peak sensitivity and complementary sensitivity, indicating a more robust design. It also reduces the amplification of measurement noise ($CS = C/(1+PC)$). Further attenuation of high-frequency measurement noise may require additional controller roll-off.

Extracting the controller

Above we simulated the closed-loop response by building closed-loop transfer functions directly with G_PS and comp_sensitivity. To obtain the corresponding controller, call observer_controller for the pure feedback controller, or extended_controller for a 2-DOF controller that takes a separate state reference.

Explicit error integration via LQI

The disturbance-model approach above achieves integral action implicitly: the Kalman filter estimates a constant disturbance, and the controller compensates for that estimate. A more direct alternative is to design a Linear-Quadratic-Integral (LQI) controller, in which output-error integrators are augmented into the plant state and the LQR cost penalizes the integrated error explicitly. The interface is lqi (gain only) and lqi_controller (full controller that takes references and measurements as inputs).

We re-use the original (un-augmented) plant G from the top of this page:

nx = G.nx
nu = G.nu
ny = G.ny

# Cost on the augmented state x_a = [x; ∫e].
# The trailing diagonal block weights the integral of the tracking error;
# increasing it makes the controller act more aggressively on accumulated error.
Q1 = cat(100.0*I(nx), 100*I(ny); dims=(1, 2))
Q2 = 0.01*I(nu)

# Observer for the un-augmented plant
R1 = 0.001I(nx)
R2 = I(ny)
K   = kalman(G, R1, R2)
obs = observer_predictor(G, K; output_state = true)

C = lqi_controller(G, obs, Q1, Q2)   # controller with inputs [r; y] and output u

NamedStateSpace{Discrete{Int64}, Float64}
A = 
 1.0                  0.0
 0.8185760897114593  -0.32715403524131614
B = 
 1.0  -1.0
 0.0   0.0019870332107124614
C = 
 3.4407477090975904  -5.17512919293522
D = 
 0.0  0.0

Sample Time: 1 (seconds)
Discrete-time state-space model
With state  names: x_int##feedback#469 x_observer##feedback#469
     input  names: y_plant_r y_plant
     output names: y_L

To inject the load disturbance from earlier (a unit step added at the plant input), we close the loop with the advanced feedback interface on named systems. Two features of that interface keep the wiring direct:

listing :u_plant in both w1 and u1 makes the plant's input serve simultaneously as the feedback control input from C and as the external summing point for the load disturbance, so no extra disturbance port has to be added to G;
omitting the reference :y_plant_r from both w2 and u2 is equivalent to r = 0 without exposing it as a closed-loop input, so the original scalar disturbance function can be passed to lsim.

G_named = named_ss(G, u = :u_plant, y = :y_plant)
Gcl = feedback(G_named, C;
        w1 = :u_plant,            # external input: load disturbance at the plant input
        u1 = :u_plant,            # feedback path drives the same physical input → both sum
        z1 = :y_plant,            # keep plant output
        # w2 = :y_plant_r,        # uncomment to also expose the reference as a closed-loop input
        u2 = :y_plant,            # C feedback input ← plant output
        z2 = :y_L,                # keep controller output (the control signal u)
        pos_feedback = true)      # `lqi_controller` already bakes in the negative-feedback sign
res = lsim(Gcl, disturbance, 100)
@test res.y[:, end] ≈ [0, -1] atol=1e-3
plot(res, ylabel = ["y" "u"]); ylims!((-0.05, 0.3), sp = 1)

The control signal again settles on -1, exactly counteracting the load disturbance. Compared to the disturbance-model design, the LQI formulation lets us tune the strength of the integral action directly through the augmented-state cost block in Q1, rather than indirectly through the disturbance-state noise covariance in R1.

2-DOF tracking with `extended_controller`

The reference-signal response of an LQG controller can be improved without adding integral action by proper reference feedforward. extended_controller returns an ExtendedStateSpace controller with inputs [xᵣ; y] and output u. The reference enters the observer dynamics through B1 = (B − KD)·L, so the observer estimate x̂ tracks the true state x even when xᵣ ≠ 0 (in contrast to a pure feedforward path that bypasses the observer).

We re-use the original plant G and build a fresh LQG problem (no disturbance model needed here):

Q1 = 100I(G.nx)
Q2 = 0.01I(G.nu)
R1 = 0.001I(G.nx)
R2 = I(G.ny)
prob_2dof = LQGProblem(G, Q1, Q2, R1, R2)

# z=[1] returns the closed-loop transfer function from xᵣ to plant output 1 as a
# second return value, useful for computing DC-gain compensation.
Ce, cl_xr_to_y = extended_controller(prob_2dof, z=[1])

(ExtendedStateSpace{Discrete{Int64}, Float64, StateSpace{Discrete{Int64}, Float64}, UnitRange{Int64}}
A = 
 0.0007987374964811885
B1 = 
 0.9032438672551932
B2 = 
 0.0019870332107124614
C2 = 
 -3.796634553679961
D21 = 
 3.796634553679961
D22 = 
 0.0
Sample Time: 1 (seconds)
Discrete-time extended state-space model, StateSpace{Discrete{Int64}, Float64}
A = 
 0.9048374180359594     -0.9032438672551932
 0.0007948132842849846   0.0007987374964811885
B = 
 0.9032438672551932
 0.9032438672551932
C = 
 0.4  0.0
D = 
 0.0

Sample Time: 1 (seconds)
Discrete-time state-space model)

The closed-loop DC gain from state reference to output is not unity in general — for xᵣ = x_ss to hold, the plant would need to contain an integrator. For this stable first-order plant we use the second return value to compute a static pre-compensation:

gain_comp = inv(dcgain(cl_xr_to_y)[1])
res = lsim(gain_comp * cl_xr_to_y, (x,t) -> min(t/10, 1), 20)
@test res.y[end] ≈ 1 atol=1e-2
plot(res, ylabel="y")

The step response settles on 1, confirming that the pre-compensated 2-DOF controller tracks unit references at DC. For plants with an integrator (e.g., a cart-position channel from a velocity-controlled actuator) dcgain(cl_xr_to_y) is already 1 and no compensation is needed.

Note, without the integral action in the controller, any error in the DC gain of the model would still lead to a steady-state error in the reference-signal response.

Integral action removes the steady-state error under model mismatch

To make the point of the previous paragraph concrete, we apply both controllers — the integrating C from lqi_controller and the non-integrating Ce from extended_controller, each designed earlier on the nominal plant G — to a perturbed plant whose gain is 20 % larger than the design model:

G_pert   = c2d(ss(tf(1.2, [10, 1])), Ts)   # 20 % larger gain than the design model G
G_pert_n = named_ss(G_pert)

# Close the reference-tracking loop directly on the named systems
#   w1 = :y_plant_r  → the reference is the external input
#   u1 = :y_plant    → wire C's feedback input to the plant output
#   z2 = G_pert_n.y  → keep only the plant output (suppress C's u as an output)
#   pos_feedback = true → lqi_controller already bakes in the negative sign of the feedback path
Gcl_pert_int = feedback(C, G_pert_n;
                    w1 = :y_plant_r,
                    z1 = Symbol[],
                    z2 = G_pert_n.y,
                    u1 = :y_plant,
                    pos_feedback = true)

# The controller includes the negative measurement-feedback sign.
Ce_named = named_ss(ss(Ce), y = C.y, u = C.u)
Gcl_pert_noint = feedback(G_pert_n, Ce_named;
                    w1 = Symbol[],
                    w2 = :y_plant_r,
                    u2 = :y_plant,
                    z1 = G_pert_n.y,
                    z2 = Symbol[],
                    pos_feedback = true)

res_int   = lsim(Gcl_pert_int,               (x,t)->min(t/10, 1), 60)
res_noint = lsim(gain_comp * Gcl_pert_noint, (x,t)->min(t/10, 1), 60)
@test res_int.y[end]   ≈ 1   atol=1e-3
@test res_noint.y[end] ≈ dcgain(gain_comp * Gcl_pert_noint)[] atol=1e-2
@test !isapprox(res_noint.y[end], 1; atol=1e-3)
plot(res_int, lab = "lqi_controller")
plot!(res_noint, ylabel = "y", lab = "extended_controller")

The integrating controller rejects the steady-state error caused by this gain mismatch. The non-integrating controller has a nonzero reference error whose magnitude depends on the plant mismatch and the feedback design.

Disturbance modeling and rejection with LQG controllers

Extracting the controller

Explicit error integration via LQI

2-DOF tracking with extended_controller

Integral action removes the steady-state error under model mismatch

2-DOF tracking with `extended_controller`