From 07993be4cfaa68b68eda4e54d628953eb4aad307 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Thu, 22 Oct 2026 09:57:31 +0000 Subject: [PATCH 1/2] Support observer-only constant disturbances in LQGProblem --- Project.toml | 2 +- docs/src/lqg_disturbance.md | 179 +++++++++++++++++------------------ src/lqg.jl | 163 ++++++++++++++++++++++++------- test/runtests.jl | 1 + test/test_lqg_disturbance.jl | 97 +++++++++++++++++++ 5 files changed, 316 insertions(+), 126 deletions(-) create mode 100644 test/test_lqg_disturbance.jl diff --git a/Project.toml b/Project.toml index 0dd3bbe0..a9c42378 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "RobustAndOptimalControl" uuid = "21fd56a4-db03-40ee-82ee-a87907bee541" authors = ["Fredrik Bagge Carlson", "Marcus Greiff"] -version = "0.4.53" +version = "0.4.54" [deps] ChainRulesCore = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" diff --git a/docs/src/lqg_disturbance.md b/docs/src/lqg_disturbance.md index 6bede18e..ba8ae298 100644 --- a/docs/src/lqg_disturbance.md +++ b/docs/src/lqg_disturbance.md @@ -1,118 +1,118 @@ # Disturbance modeling and rejection with LQG controllers -This example will demonstrate how you can add disturbance models to ta plant model and achieve effective disturbance rejection using an LQG controller. For simplicity, we will consider a simple first-order system $G$ +A constant unmeasured input disturbance can be estimated by augmenting the observer +model with constant disturbance variables. The physical feedback gain is designed +on the plant, and the disturbance estimate is subtracted from the control input. +For the scalar plant ```math -\begin{aligned} -\dot{x} &= -ax + b(u + d) \\ -y &= cx -\end{aligned} +\dot{x} = -ax + b(u+d), \qquad y = cx, ``` +the augmented observer includes a variable with dynamics ``\dot{d}=0`` in continuous +time, or ``d^+=d`` in discrete time. This variable is uncontrollable and is excluded +from the state-feedback Riccati equation. -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 andy disturbance model. For details regarding the setup of an LQG problem, see, the [`LQGProblem`](@ref) documentation. - -We start by defining the process model and discretize it using zero-order hold. +## Physical plant and baseline design ```@example LQG_DIST -using RobustAndOptimalControl, ControlSystemsBase, Plots, LinearAlgebra -Ts = 1 # Sample time -G = c2d(ss(tf(1, [10, 1])), Ts) # Process model +using RobustAndOptimalControl, ControlSystemsBase, Plots, LinearAlgebra, Test +Ts = 1 +G = c2d(ss(tf(1, [10, 1])), Ts) +nx, nu, ny = G.nx, G.nu, G.ny +Q1 = 100I(ny) +Q2 = 0.01I(nu) +R1 = 0.001I(nx) +R2 = I(ny) +Pe = ExtendedStateSpace(G; B1=I, C1=G.C) +prob = LQGProblem(Pe, Q1, Q2, R1, R2) +disturbance = (x, t) -> t * Ts ≥ 10 +plot(lsim(G_PS(prob), disturbance, 100)) ``` -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`` -```@example LQG_DIST -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 +The baseline observer does not model the constant disturbance, so its estimate is +biased under a sustained load. -R1 = 0.001I(nx) # State noise covariance -R2 = I(ny) # measurement noise covariance -prob = LQGProblem(G, Q1, Q2, R1, R2) +## Exact constant-disturbance model -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 +```@example LQG_DIST +Gd = add_low_frequency_disturbance(G) +Pe = ExtendedStateSpace(Gd; B1=I, C1=Gd.C) +disturbance_indices = collect((G.nx+1):Gd.nx) +disturbance_input_indices = collect(1:nu) +R1 = Diagonal([0.001, 1.0]) +prob = LQGProblem(Pe, Q1, Q2, R1, R2; + disturbance_indices, disturbance_input_indices) + +L = lqr(prob) +@test L[:, disturbance_indices] ≈ I(nu) +@test isstable(feedback(G, observer_controller(prob))) +@test norm(dcgain(G_PS(prob))) < 1e-8 +Gcl = [G_PS(prob); -input_comp_sensitivity(prob)] res = lsim(Gcl, disturbance, 100) -plot(res) +plot(res, ylabel=["y" "u"]) ``` -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. +`disturbance_indices` refers to augmented state-variable indices, whereas +`disturbance_input_indices` refers to control-input indices. Their ordering defines +the pairing. For a subset of inputs, pass their indices and augment the observer +using the corresponding columns of `G.B`. Model reduction must precede this +augmentation so that the disturbance coordinates remain identifiable. -The next step is to add a disturbance model to the plant model. Since the disturbance if of low-frequency character (indeed, its transfer function is $1/s$), we make use of the function `add_low_frequency_disturbance` +If the input indices are omitted, the input mapping is inferred when the physical +input matrix has full column rank and exactly represents the disturbance coupling. +Redundant actuator configurations require an explicit input pairing. Unmatched +disturbances and direct disturbance contributions to outputs are unsupported by +this interface. -```@example LQG_DIST -Gd = add_low_frequency_disturbance(G, ϵ = 1e-6) # The ϵ moves the integrator pole slightly into the stable region -nx = Gd.nx -``` - -There is no point trying to penalize the disturbance state in the LQR problem, it's not controllable, we thus penalize the output only, which we can write as - -```math -y^T Q_1 y = (Cx)^T Q_1 Cx = x^T (C^T Q_1C) x -``` +The Kalman filter uses the complete augmented model. The covariance of each +disturbance variable determines the estimator response to changing disturbances. +The controller implements ``u=-L_x\hat{x}-\hat{d}``; its disturbance compensation +gain is specified independently of the physical LQR optimization. ```@example LQG_DIST -C = Gd.C -Q1 = 100C'diagm(ones(G.nx)) * C # state cost matrix -x0 = zeros(nx) -nothing # hide -``` - -We also provide new covariance matrices for the Kalman filter where the entry of the state-covariance matrix that corresponds to the disturbance state (the second and last state) determines how fast the Kalman filter integrates the disturbance. We choose a large value (1), implying fast integration - -```@example LQG_DIST -R1 = diagm([0.001, 1]) -R2 = I(ny) -SQ = [zeros(nx-nu, nu); Q2] # Adding Q2 to the x-u cross term achieves perfect integral action without manually modifying the computed feedback gain, if not all inputs are augmented with integral action, we should add Q2[augmented_inds, :] instead -prob = LQGProblem(Gd, Q1, Q2, R1, R2; SQ=SQ) -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) +w = exp10.(LinRange(-3, log10(pi / Ts), 200)) +gangoffourplot(prob, w, lab="", legend=:bottomright) ``` -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. +A smaller disturbance covariance reduces the estimator response rate and changes +the trade-off between disturbance rejection and measurement-noise amplification. ```@example LQG_DIST -using Test -@test lqr(prob)[end] ≈ 1.0 # The gain from estimated disturbance state to control signal should be 1.0, indicating perfect integral action +R1 = Diagonal([0.001, 0.2]) +prob = LQGProblem(Pe, Q1, Q2, R1, R2; + disturbance_indices, disturbance_input_indices) +Gcl = [G_PS(prob); -input_comp_sensitivity(prob)] +f1 = plot(lsim(Gcl, disturbance, 100), ylabel=["y" "u"]) +f2 = gangoffourplot(prob, w, lab="", legend=:bottomright) +plot(f1, f2) ``` -Before we feel confident about deploying the LQG controller, we investigate its closed-loop properties. +## Controller and reference response -```@example LQG_DIST -w = exp10.(LinRange(-3, log10(pi / Ts), 200)) -gangoffourplot(prob, w, lab = "", legend = :bottomright) -``` +`observer_controller(prob)` returns the feedback controller with the standard +negative-feedback convention. `extended_controller(prob)` provides state-reference +and measurement inputs with the feedback sign included in the controller. -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 state. Let's see how a slightly more conservative design fares: +The physical plant is returned by `system_mapping(prob)`. The augmented model is +available through `prob.sys`; `performance_mapping(prob)` and `lft(prob)` retain +its disturbance channels. Physical closed-loop stability must be assessed without +including the autonomous disturbance generator as a plant mode. ```@example LQG_DIST -R1 = diagm([0.001, 0.2]) # Reduce the noise on the integrator state from 1 to 0.2 -R2 = I(ny) -prob = LQGProblem(Gd, Q1, Q2, R1, R2) - -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) +Cfb = observer_controller(prob) +Cff = ff_controller(prob) +Gref = closedloop(prob) * static_gain_compensation(prob) +@test isstable(Gref) +@test dcgain(Gref) ≈ I(ny) +plot(step(Gref, 100)) ``` -We see that we now have a slightly larger disturbance response than before, but in exchange, we lowered the peak sensitivity and complimentary sensitivity from (1.51, 1.25) to (1.31, 1.11), a more robust design. We also reduced the amplification of measurement noise ($CS = C/(1+PC)$). To be really happy with the design, we should probably add high-frequency roll-off as well. - -## Extracting the controller - -Above we simulated the closed-loop response by building closed-loop transfer functions directly with [`G_PS`](@ref) and [`comp_sensitivity`](@ref). To obtain the controller used under the hood, call [`observer_controller`](@ref) for the pure feedback controller, or [`extended_controller`](@ref) for a 2-DOF controller that takes a separate state reference. +`G_PS(prob)*Cff` has the same reference transfer function but may contain canceling +controller modes in its realization. `Gref` avoids these redundant modes. ## Explicit error integration via LQI -The disturbance-model approach above achieves integral action implicitly: the Kalman filter estimates a slow disturbance state, and the LQR gain on that state acts as an integrator. 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`](@ref) (gain only) and [`lqi_controller`](@ref) (full controller that takes references and measurements as inputs). +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`](@ref) (gain only) and [`lqi_controller`](@ref) (full controller that takes references and measurements as inputs). We re-use the original (un-augmented) plant `G` from the top of this page: @@ -210,28 +210,23 @@ Gcl_pert_int = feedback(C, G_pert_n; u1 = :y_plant, pos_feedback = true) -# Non-integrating closed loop with `Ce` from `extended_controller`. The wiring mirrors what -# `extended_controller(prob, z=[1])` does internally to build `cl_xr_to_y` — plant first, -# default `pos_feedback`, so the same `gain_comp` we computed earlier still applies. -# w1 = Symbol[] → plant has no external input (control comes from the controller) -# w2 = :y_plant_r → state reference is the external input -# u2 = :y_plant → Ce's feedback input is wired to the plant output -# z1 = G_pert_n.y → keep the plant output -# z2 = Symbol[] → suppress Ce's u as an output +# 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[]) + 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] ≈ 1.2 atol=1e-2 # 20 % gain mismatch leaks straight through +@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 still settles on `1` despite the controller having been designed for a different plant — the error integrator absorbs the model mismatch. The non-integrating 2-DOF controller settles on `1.2`, in proportion to the gain mismatch between the design model and the real plant. +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. diff --git a/src/lqg.jl b/src/lqg.jl index ea3d5ba3..381568db 100644 --- a/src/lqg.jl +++ b/src/lqg.jl @@ -3,7 +3,7 @@ import Base.getindex import ControlSystemsBase.numeric_type """ - G = LQGProblem(sys::ExtendedStateSpace, Q1, Q2, R1, R2; qQ=0, qR=0, SQ=nothing, SR=nothing) + G = LQGProblem(sys::ExtendedStateSpace, Q1, Q2, R1, R2; qQ=0, qR=0, SQ=nothing, SR=nothing, disturbance_indices=Int[], disturbance_input_indices=nothing) Return an LQG object that describes the closed control loop around the process `sys=ss(A,B,C,D)` where the controller is of LQG-type. The controller is specified by weight matrices `Q1,Q2` @@ -67,6 +67,36 @@ Several functions are defined for instances of `LQGProblem` - [`lqr`](@ref) - [`observer_controller`](@ref) +## Observer-only constant disturbances + +Set `disturbance_indices` to the indices of constant disturbance variables in the +augmented state vector. These variables must be autonomous, have zero dynamics in +continuous time or identity dynamics in discrete time, and have no direct contribution +to `C1` or `C2`. The physical subsystem must satisfy the usual LQR assumptions, and +the augmented observer model must satisfy the Kalman-filter assumptions. + +`disturbance_input_indices` pairs each disturbance variable with a control input. +With physical indices `x` and disturbance indices `d`, the constructor verifies +`A[x, d] ≈ B2[x, :] * E`, where `E` selects the specified input columns. If input +indices are omitted, `E` is inferred only when `B2[x, :]` has full column rank and +the disturbance is matched. Unmatched disturbances and ambiguous input allocations +require a different controller formulation. + +`lqr(l)` solves the physical LQR problem and returns the full gain with +`L[:, x] = Lx` and `L[:, d] = E`, implementing `u = -Lx*x̂ - E*d̂`. +The physical rows of `SQ` are supported; disturbance rows must be zero. This is a +physical LQR design with explicit disturbance compensation, not a stabilizing LQR +solution for the uncontrollable augmented model. `kalman(l)` and the observer +methods use the complete augmented model. + +`system_mapping(l)`, the sensitivity functions, and `closedloop(l)` exclude the +autonomous disturbance generator from the physical plant. `l.sys`, +`performance_mapping(l)`, and `lft(l)` retain the modeled disturbance channels. +Reference gain compensation uses the physical equilibrium equations in the selected +time domain. A separately realized product `G_PS(l)*ff_controller(l)` can contain +canceling controller poles; use `closedloop(l)*static_gain_compensation(l)` for the +reference response without those redundant modes. + A video tutorial on how to use the LQG interface is available [here](https://youtu.be/NuAxN1mGCPs) ## Introduction of references @@ -84,8 +114,13 @@ struct LQGProblem qR::Real SQ::AbstractMatrix SR::AbstractMatrix + disturbance_indices::Vector{Int} + disturbance_gain::AbstractMatrix end +LQGProblem(sys::ExtendedStateSpace, Q1, Q2, R1, R2, qQ, qR, SQ, SR) = + LQGProblem(sys, Q1, Q2, R1, R2, qQ, qR, SQ, SR, Int[], zeros(eltype(sys.A), size(sys.B2, 2), 0)) + ControlSystemsBase.isdiscrete(l::LQGProblem) = ControlSystemsBase.isdiscrete(l.sys) ControlSystemsBase.iscontinuous(l::LQGProblem) = ControlSystemsBase.iscontinuous(l.sys) @@ -99,7 +134,8 @@ function LQGProblem( qR = 0, SQ = nothing, SR = nothing, - kwargs..., + disturbance_indices = Int[], + disturbance_input_indices = nothing, ) if sys isa ExtendedStateSpace iszero(sys.D12) || error("When providing quadratic penatly matrices, non-zero D12 is not supported since it is not clear how to interpret the control input cost. Consider using the H2 interface instead.") @@ -118,9 +154,51 @@ function LQGProblem( SQ === nothing && (SQ = zeros(size(sys.B2))) SR === nothing && (SR = zeros(size(sys.C2'))) - LQGProblem(sys, Q1, Q2, R1, R2, qQ, qR, SQ, SR; kwargs...) + d, E = _disturbance_feedback(sys, disturbance_indices, disturbance_input_indices) + isempty(d) || iszero(SQ[d, :]) || throw(ArgumentError("SQ must be zero on disturbance variables; their compensation is specified by the disturbance mapping")) + LQGProblem(sys, Q1, Q2, R1, R2, qQ, qR, SQ, SR, d, E) end +function _disturbance_feedback(sys, disturbance_indices, input_indices) + d = collect(Int, disturbance_indices) + length(unique(d)) == length(d) || throw(ArgumentError("disturbance_indices must be unique")) + all(i -> 1 <= i <= sys.nx, d) || throw(ArgumentError("disturbance_indices must index the augmented state vector")) + x = setdiff(1:sys.nx, d) + nu = size(sys.B2, 2) + if isempty(d) + input_indices === nothing || isempty(input_indices) || throw(ArgumentError("disturbance_input_indices requires disturbance_indices")) + return d, zeros(eltype(sys.A), nu, 0) + end + isempty(x) && throw(ArgumentError("At least one physical state variable is required")) + iszero(sys.A[d, x]) && iszero(sys.B2[d, :]) || throw(ArgumentError("Disturbance variables must be autonomous and uncontrollable")) + Ad = isdiscrete(sys) ? I(length(d)) : zeros(length(d), length(d)) + sys.A[d, d] == Ad || throw(ArgumentError("Disturbance dynamics must be constant: zero in continuous time or identity in discrete time")) + iszero(sys.C1[:, d]) && iszero(sys.C2[:, d]) || throw(ArgumentError("Direct disturbance contributions to performance or measured outputs are not supported")) + Bu, Bd = sys.B2[x, :], sys.A[x, d] + if input_indices === nothing + rank(Bu) == nu || throw(ArgumentError("The disturbance input mapping is not unique; provide disturbance_input_indices")) + E = Bu \ Bd + else + inds = collect(Int, input_indices) + length(inds) == length(d) || throw(ArgumentError("disturbance_input_indices must have one entry per disturbance variable")) + all(i -> 1 <= i <= nu, inds) || throw(ArgumentError("disturbance_input_indices must index control inputs")) + E = Matrix{eltype(Bu)}(I, nu, nu)[:, inds] + end + isapprox(Bu * E, Bd) || throw(ArgumentError("Disturbances must be matched to the specified control inputs: B2[x, :] * E must equal A[x, d]")) + d, E +end + +_physical_indices(l::LQGProblem) = setdiff(1:l.sys.nx, l.disturbance_indices) +_observer_model(l::LQGProblem) = system_mapping(l.sys, identity) + +function _physical_model(l::LQGProblem) + P = _observer_model(l) + isempty(l.disturbance_indices) && return P + x = _physical_indices(l) + G = ss(P.A[x, x], P.B[x, :], P.C[:, x], P.D, P.timeevol) + P isa NamedStateSpace ? named_ss(G; x=P.x[x], u=P.u, y=P.y, name=P.name) : G +end + """ LQGProblem(P::ExtendedStateSpace) @@ -186,7 +264,13 @@ end function ControlSystemsBase.lqr(l::LQGProblem) (; A, B2, C1, Q1, qQ, C2, Q2, SQ) = l - L = lqr(l.timeevol, A, B2, Hermitian(C1'Q1*C1 + qQ * C2'C2), Q2, SQ) + x = _physical_indices(l) + Q = C1'Q1*C1 + qQ * C2'C2 + Lx = lqr(l.timeevol, A[x, x], B2[x, :], Hermitian(Q[x, x]), Q2, SQ[x, :]) + L = zeros(promote_type(eltype(Lx), eltype(l.disturbance_gain)), size(B2, 2), size(A, 1)) + L[:, x] = Lx + L[:, l.disturbance_indices] = l.disturbance_gain + L end """ @@ -263,8 +347,12 @@ dcgain(closedloop(G)*Lr) ≈ I ``` """ function static_gain_compensation(l::LQGProblem, L = lqr(l)) - @unpack A, C1, B1, B2, D11, D12 = l - pinv(D12 - (C1 - D12*L) * inv(A - B2*L) * B2) + x = _physical_indices(l) + A, B, C, D = l.A[x, x], l.B2[x, :], l.C1[:, x], l.D12 + Lx = L[:, x] + Acl = A - B*Lx + equilibrium = isdiscrete(l) ? I - Acl : -Acl + pinv(D + (C - D*Lx) * (equilibrium \ B)) end function static_gain_compensation(A, B, C, D, L) @@ -329,17 +417,20 @@ function extended_controller(P::AbstractStateSpace, L::AbstractMatrix, K::Abstra if z === nothing return Ce0 end - # `feedback` defaults to pos_feedback=false; combined with D21 = L and C2 = -L this gives - # u = L(xᵣ − x̂) on the wire. U2 = (1:ny) .+ nx selects the y-slice of the controller input [xᵣ; y]. + # The measurement-to-control channel already includes the negative feedback sign. Ce = ss(Ce0) - cl = feedback(P, Ce, Z1 = z, Z2=[], U2=(1:ny) .+ nx, Y1 = :, W2=1:nx, W1=[]) + cl = feedback(P, Ce, Z1 = z, Z2=[], U2=(1:ny) .+ nx, Y1 = :, W2=1:nx, W1=[], pos_feedback=true) Ce0, cl end -function extended_controller(l::LQGProblem, L::AbstractMatrix = lqr(l), K::AbstractMatrix = kalman(l); kwargs...) - P = system_mapping(l, identity) # identity skips the optional plant transform - extended_controller(P, L, K; kwargs...) +function extended_controller(l::LQGProblem, L::AbstractMatrix = lqr(l), K::AbstractMatrix = kalman(l); z=nothing, kwargs...) + Ce = extended_controller(_observer_model(l), L, K; kwargs...) + z === nothing && return Ce + P = _physical_model(l) + nx = l.sys.nx + cl = feedback(P, ss(Ce), Z1=z, Z2=[], U2=(1:P.ny) .+ nx, Y1=:, W2=1:nx, W1=[], pos_feedback=true) + Ce, cl end """ @@ -353,12 +444,13 @@ Note: the transfer function returned is only a representation of the controller See also [`ff_controller`](@ref) that generates ``C_{ff}``. """ -function ControlSystemsBase.observer_controller(l::LQGProblem, L::AbstractMatrix = lqr(l), K::Union{AbstractMatrix, Nothing} = nothing; direct = false) - sys = system_mapping(l, identity) +function ControlSystemsBase.observer_controller(l::LQGProblem, L::AbstractMatrix = lqr(l), K::Nothing = nothing; direct = false) + observer_controller(l, L, kalman(l; direct); direct) +end + +function ControlSystemsBase.observer_controller(l::LQGProblem, L::AbstractMatrix, K::AbstractMatrix; direct = false) + sys = _observer_model(l) A,B,C,D = ssdata(sys) - if K === nothing - K = kalman(l; direct) - end if direct && isdiscrete(sys) iszero(D) || throw(ArgumentError("D must be zero when using direct formulation of `observer_controller`")) IKC = (I - K*C) @@ -385,7 +477,7 @@ function ControlSystemsBase.observer_controller(l::LQGProblem, L::AbstractMatrix end function ControlSystemsBase.observer_predictor(l::LQGProblem, K::Union{AbstractMatrix, Nothing} = nothing; direct = false, kwargs...) - P = system_mapping(l, identity) + P = _observer_model(l) if K === nothing K = kalman(l; direct) end @@ -393,7 +485,7 @@ function ControlSystemsBase.observer_predictor(l::LQGProblem, K::Union{AbstractM end function ControlSystemsBase.observer_filter(l::LQGProblem, K = kalman(l); kwargs...) - P = system_mapping(l, identity) + P = _observer_model(l) observer_filter(P, K; kwargs...) end @@ -436,7 +528,7 @@ function ff_controller(sys::AbstractStateSpace, L, K, Lr = nothing; comp_dc = tr end function ff_controller(l::LQGProblem, L = lqr(l), K = kalman(l); kwargs...) - ff_controller(system_mapping(l, identity), L, K, static_gain_compensation(l, L); kwargs...) + ff_controller(_observer_model(l), L, K, static_gain_compensation(l, L); kwargs...) end """ @@ -449,23 +541,27 @@ The return value will be the closed loop from filtred reference only, other dist Use `static_gain_compensation` to adjust the gain from references acting on the input B2, `dcgain(closedloop(l))*static_gain_compensation(l) ≈ I` """ function closedloop(l::LQGProblem, L = lqr(l), K = kalman(l)) - # todo: reimplement as lft - P = system_mapping(l, identity) - @unpack A, B1, B2, C2, C1 = l - n = P.nx - Acl = [A-B2*L B2*L; zero(A) A-K*C2] # 8.28 - #Glad Ljung has B2 here instead of B1. The difference lies in Glad, Ljung calling the system from references acting through B2 the closed loop, whereas most other literature uses lft(P, K) as the closed loop, i.e., from B1 - Bcl = [B2; zero(B2)] - - Ccl = [C1 zero(C1)] - syscl = ss(Acl, Bcl, Ccl, 0, l.timeevol) + @unpack A, B2, C2, C1 = l + x = _physical_indices(l) + B = B2[x, :] + Acl = [A[x, x]-B*L[:, x] B*L; zeros(eltype(A), size(A, 1), length(x)) A-K*C2] + Bcl = [B; zero(B2)] + Ccl = [C1[:, x] zeros(eltype(C1), size(C1, 1), size(A, 1))] + ss(Acl, Bcl, Ccl, 0, l.timeevol) end ControlSystemsBase.lft(l::LQGProblem) = lft(l.sys, -observer_controller(l)) -system_mapping(l::LQGProblem, args...) = system_mapping(l.sys, args...) +""" + system_mapping(l::LQGProblem, transform=sminreal) + +Return the physical control-input to measured-output model, excluding the variables +listed in `disturbance_indices`. The augmented observer model is available through +`system_mapping(l.sys, identity)`. +""" +system_mapping(l::LQGProblem, transform=sminreal) = transform(_physical_model(l)) performance_mapping(l::LQGProblem, args...) = performance_mapping(l.sys, args...) @@ -505,14 +601,14 @@ d₁────+──┴──► P ├─────┬──►e₄ G_PS(l::LQGProblem) """ function G_PS(l::LQGProblem) # Load disturbance to output - return output_sensitivity(l) * system_mapping(l) + return G_PS(system_mapping(l), observer_controller(l)) end """ G_CS(l::LQGProblem) """ function G_CS(l::LQGProblem) # Noise to control signal - return input_sensitivity(l) * observer_controller(l) + return G_CS(system_mapping(l), observer_controller(l)) end # loopgain(P,C) = P*C @@ -877,6 +973,7 @@ function lqi_controller(G, obs, Q1, Q2, args...; integrator_outputs=1:G.ny, ϵ=0 end function lqi_controller(prob::LQGProblem, Qi::AbstractMatrix; integrator_outputs=1:prob.sys.ny, ϵ=0) + isempty(prob.disturbance_indices) || throw(ArgumentError("lqi_controller does not support observer-only disturbance variables; use observer_controller for disturbance-based integral action")) G = system_mapping(prob, identity) K = kalman(prob) obs = observer_predictor(G, K; output_state=true) diff --git a/test/runtests.jl b/test/runtests.jl index 1b631ccf..89225ca5 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -63,6 +63,7 @@ using Test @testset "LQG" begin @info "Testing LQG" include("test_lqg.jl") + include("test_lqg_disturbance.jl") end @testset "LQI" begin diff --git a/test/test_lqg_disturbance.jl b/test/test_lqg_disturbance.jl new file mode 100644 index 00000000..d14f5b7e --- /dev/null +++ b/test/test_lqg_disturbance.jl @@ -0,0 +1,97 @@ +using RobustAndOptimalControl, ControlSystemsBase, LinearAlgebra, Test + +@testset "Observer-only constant disturbances" begin + for discrete in (false, true) + P = ss(tf(1, [1, 1, 0])) + discrete && (P = c2d(P, 0.2)) + Pa = add_low_frequency_disturbance(P) + pe = ExtendedStateSpace(Pa; B1=I, C1=[P.C zeros(P.ny, P.nu)]) + prob = LQGProblem(pe, [2.0], [0.3], ones(Pa.nx), [0.1]; + disturbance_indices=[Pa.nx], disturbance_input_indices=[1], qQ=0.2, qR=0.1) + L, K = lqr(prob), kalman(prob) + Lx = lqr(P, 2.2P.C'P.C, [0.3;;]) + @test L[:, 1:P.nx] ≈ Lx + @test L[:, end:end] == ones(1, 1) + @test system_mapping(prob, identity).nx == P.nx + @test observer_predictor(prob).nx == Pa.nx + @test observer_filter(prob).nx == Pa.nx + Cfb = observer_controller(prob) + manual = observer_controller(Pa, [Lx ones(1, 1)], K) + @test freqresp(Cfb, [0.1, 1, 10]) ≈ freqresp(manual, [0.1, 1, 10]) + @test isstable(feedback(P, Cfb)) + @test norm(dcgain(G_PS(prob))) < 1e-8 + @test norm(step(G_PS(prob), 100).y[:, end, :]) < 1e-6 + @test isstable(closedloop(prob)) + @test dcgain(closedloop(prob)*static_gain_compensation(prob)) ≈ ones(1, 1) + Cff = ff_controller(prob) + @test dcgain(minreal(feedback(P, Cfb)*Cff, 1e-7)) ≈ ones(1, 1) + @test freqresp(closedloop(prob), [0.1, 1, 10]) ≈ freqresp(feedback(P, Cfb)*ff_controller(prob; comp_dc=false), [0.1, 1, 10]) + for (actual, expected) in zip(gangoffour(prob), gangoffour(P, Cfb)) + @test freqresp(actual, [0.1, 1, 10]) ≈ freqresp(expected, [0.1, 1, 10]) + end + Ce, cl = extended_controller(prob; z=[1]) + @test isstable(cl) + @test freqresp(system_mapping(Ce), [0.1, 1]) ≈ -freqresp(Cfb, [0.1, 1]) + @test size(lft(prob)) == (1, Pa.nx) + @test freqresp(lft(prob), [0.1, 1]) ≈ freqresp(lft(pe, -Cfb), [0.1, 1]) + @test_throws ArgumentError lqi_controller(prob, ones(1, 1)) + @test performance_mapping(prob).nu == Pa.nx + inferred = LQGProblem(pe, [2.0], [0.3], ones(Pa.nx), [0.1]; disturbance_indices=[Pa.nx], qQ=0.2) + @test lqr(inferred) ≈ L + if discrete + Cd = observer_controller(prob; direct=true) + @test isstable(feedback(P, Cd)) + @test norm(dcgain(G_PS(P, Cd))) < 1e-8 + end + end + + @testset "MIMO ordering and explicit allocation" begin + P = ss([-1.0 0; 0 -2], [1.0 0; 0 2], Matrix{Float64}(I, 2, 2), 0) + E = [0.0 1; 1 0] + Pa = add_low_frequency_disturbance(P, P.B*E) + perm = [3, 1, 4, 2] + Pa = ss(Pa.A[perm, perm], Pa.B[perm, :], Pa.C[:, perm], Pa.D) + pe = ExtendedStateSpace(Pa; B1=I) + SQ = zeros(4, 2); SQ[[2, 4], :] = 0.1I(2) + prob = LQGProblem(pe, [1.0, 2], [0.2, 0.4], ones(4), ones(2); + disturbance_indices=[1, 3], disturbance_input_indices=[2, 1], SQ, qQ=0.3) + @test lqr(prob)[:, [1, 3]] == E + @test lqr(prob)[:, [2, 4]] ≈ lqr(P, Diagonal([1.3, 2.3]), Diagonal([0.2, 0.4]), SQ[[2, 4], :]) + @test isstable(feedback(P, observer_controller(prob))) + @test norm(dcgain(G_PS(prob))) < 1e-8 + @test dcgain(closedloop(prob)*static_gain_compensation(prob)) ≈ I(2) + + for kwargs in ((disturbance_indices=[1, 1],), (disturbance_indices=[5],), + (disturbance_indices=[1, 3], disturbance_input_indices=[1]), + (disturbance_indices=[1, 3], disturbance_input_indices=[1, 3]), + (disturbance_indices=[1, 3], disturbance_input_indices=[1, 2])) + @test_throws ArgumentError LQGProblem(pe, ones(2), ones(2), ones(4), ones(2); kwargs...) + end + A = copy(pe.A); A[1, 1] = -0.001 + @test_throws ArgumentError LQGProblem(ExtendedStateSpace(pe; A), ones(2), ones(2), ones(4), ones(2); disturbance_indices=[1, 3]) + C1 = copy(pe.C1); C1[1, 1] = 1 + @test_throws ArgumentError LQGProblem(ExtendedStateSpace(pe; C1), ones(2), ones(2), ones(4), ones(2); disturbance_indices=[1, 3]) + + + for overrides in ( + (; A=pe.A + [1.0; 0; 0; 0] * [0.0 1 0 0]), + (; B2=pe.B2 + [1.0; 0; 0; 0] * [1.0 0]), + (; C2=pe.C2 + [1.0; 0] * [1.0 0 0 0]), + ) + bad = ExtendedStateSpace(pe; overrides...) + @test_throws ArgumentError LQGProblem(bad, ones(2), ones(2), ones(4), ones(2); disturbance_indices=[1, 3]) + end + @test_throws ArgumentError LQGProblem(pe, ones(2), ones(2), ones(4), ones(2); disturbance_indices=[1, 3], SQ=ones(4, 2)) + @test_throws ArgumentError LQGProblem(pe, ones(2), ones(2), ones(4), ones(2); disturbance_input_indices=[1]) + unmatched = ss([-1.0 0; 0 -2], [1.0; 0;;], Matrix{Float64}(I, 2, 2), 0) + unmatched = add_low_frequency_disturbance(unmatched, [0.0; 1;;]) + @test_throws ArgumentError LQGProblem(ExtendedStateSpace(unmatched; B1=I), ones(2), ones(1), ones(3), ones(2); disturbance_indices=[3]) + + redundant = ss([-1.0;;], [1.0 1], [1.0;;], 0) + Pa = add_low_frequency_disturbance(redundant, redundant.B[:, 2:2]) + pe = ExtendedStateSpace(Pa; B1=I) + @test_throws ArgumentError LQGProblem(pe, ones(1), ones(2), ones(2), ones(1); disturbance_indices=[2]) + prob = LQGProblem(pe, ones(1), ones(2), ones(2), ones(1); disturbance_indices=[2], disturbance_input_indices=[2]) + @test lqr(prob)[:, 2] == [0, 1] + end +end From 615ebd42956ee0d33234341889624848a128180c Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Thu, 22 Oct 2026 10:29:17 +0000 Subject: [PATCH 2/2] Preserve the disturbance tutorial progression and clarify compensation helper --- docs/src/lqg_disturbance.md | 160 +++++++++++++++++++----------------- src/lqg.jl | 4 +- 2 files changed, 85 insertions(+), 79 deletions(-) diff --git a/docs/src/lqg_disturbance.md b/docs/src/lqg_disturbance.md index ba8ae298..4789c8ee 100644 --- a/docs/src/lqg_disturbance.md +++ b/docs/src/lqg_disturbance.md @@ -1,114 +1,120 @@ # Disturbance modeling and rejection with LQG controllers -A constant unmeasured input disturbance can be estimated by augmenting the observer -model with constant disturbance variables. The physical feedback gain is designed -on the plant, and the disturbance estimate is subtracted from the control input. +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$ -For the scalar plant ```math -\dot{x} = -ax + b(u+d), \qquad y = cx, +\begin{aligned} +\dot{x} &= -ax + b(u + d) \\ +y &= cx +\end{aligned} ``` -the augmented observer includes a variable with dynamics ``\dot{d}=0`` in continuous -time, or ``d^+=d`` in discrete time. This variable is uncontrollable and is excluded -from the state-feedback Riccati equation. -## Physical plant and baseline design +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`](@ref) documentation. + +We start by defining the process model and discretize it using zero-order hold. ```@example LQG_DIST -using RobustAndOptimalControl, ControlSystemsBase, Plots, LinearAlgebra, Test -Ts = 1 -G = c2d(ss(tf(1, [10, 1])), Ts) -nx, nu, ny = G.nx, G.nu, G.ny -Q1 = 100I(ny) -Q2 = 0.01I(nu) -R1 = 0.001I(nx) -R2 = I(ny) -Pe = ExtendedStateSpace(G; B1=I, C1=G.C) -prob = LQGProblem(Pe, Q1, Q2, R1, R2) -disturbance = (x, t) -> t * Ts ≥ 10 -plot(lsim(G_PS(prob), disturbance, 100)) +using RobustAndOptimalControl, ControlSystemsBase, Plots, LinearAlgebra +Ts = 1 # Sample time +G = c2d(ss(tf(1, [10, 1])), Ts) # Process model ``` -The baseline observer does not model the constant disturbance, so its estimate is -biased under a sustained load. +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`` +```@example LQG_DIST +nx = G.nx +nu = G.nu +ny = G.ny +x0 = zeros(G.nx) # Initial condition -## Exact constant-disturbance model +Q1 = 100diagm(ones(G.nx)) # state cost matrix +Q2 = 0.01diagm(ones(nu)) # control cost matrix -```@example LQG_DIST -Gd = add_low_frequency_disturbance(G) -Pe = ExtendedStateSpace(Gd; B1=I, C1=Gd.C) -disturbance_indices = collect((G.nx+1):Gd.nx) -disturbance_input_indices = collect(1:nu) -R1 = Diagonal([0.001, 1.0]) -prob = LQGProblem(Pe, Q1, Q2, R1, R2; - disturbance_indices, disturbance_input_indices) +R1 = 0.001I(nx) # State noise covariance +R2 = I(ny) # measurement noise covariance +prob = LQGProblem(G, Q1, Q2, R1, R2) -L = lqr(prob) -@test L[:, disturbance_indices] ≈ I(nu) -@test isstable(feedback(G, observer_controller(prob))) -@test norm(dcgain(G_PS(prob))) < 1e-8 -Gcl = [G_PS(prob); -input_comp_sensitivity(prob)] +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, ylabel=["y" "u"]) +plot(res) ``` -`disturbance_indices` refers to augmented state-variable indices, whereas -`disturbance_input_indices` refers to control-input indices. Their ordering defines -the pairing. For a subset of inputs, pass their indices and augment the observer -using the corresponding columns of `G.B`. Model reduction must precede this -augmentation so that the disturbance coordinates remain identifiable. +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. -If the input indices are omitted, the input mapping is inferred when the physical -input matrix has full column rank and exactly represents the disturbance coupling. -Redundant actuator configurations require an explicit input pairing. Unmatched -disturbances and direct disturbance contributions to outputs are unsupported by -this interface. +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` -The Kalman filter uses the complete augmented model. The covariance of each -disturbance variable determines the estimator response to changing disturbances. -The controller implements ``u=-L_x\hat{x}-\hat{d}``; its disturbance compensation -gain is specified independently of the physical LQR optimization. +```@example LQG_DIST +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: + +```math +y^T Q_1 y = (Cx)^T Q_1 Cx = x^T (C^T Q_1C) x +``` ```@example LQG_DIST -w = exp10.(LinRange(-3, log10(pi / Ts), 200)) -gangoffourplot(prob, w, lab="", legend=:bottomright) +C = Gd.C +Q1 = 100diagm(ones(ny)) # output cost matrix +Pe = ExtendedStateSpace(Gd; B1=I, C1=C) +x0 = zeros(nx) +nothing # hide ``` -A smaller disturbance covariance reduces the estimator response rate and changes -the trade-off between disturbance rejection and measurement-noise amplification. +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 ```@example LQG_DIST -R1 = Diagonal([0.001, 0.2]) +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); -input_comp_sensitivity(prob)] -f1 = plot(lsim(Gcl, disturbance, 100), ylabel=["y" "u"]) -f2 = gangoffourplot(prob, w, lab="", legend=:bottomright) -plot(f1, f2) +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) ``` -## Controller and reference response +`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. -`observer_controller(prob)` returns the feedback controller with the standard -negative-feedback convention. `extended_controller(prob)` provides state-reference -and measurement inputs with the feedback sign included in the controller. +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. -The physical plant is returned by `system_mapping(prob)`. The augmented model is -available through `prob.sys`; `performance_mapping(prob)` and `lft(prob)` retain -its disturbance channels. Physical closed-loop stability must be assessed without -including the autonomous disturbance generator as a plant mode. +```@example LQG_DIST +using Test +@test lqr(prob)[:, disturbance_indices] ≈ I(nu) +``` + +Before we feel confident about deploying the LQG controller, we investigate its closed-loop properties. ```@example LQG_DIST -Cfb = observer_controller(prob) -Cff = ff_controller(prob) -Gref = closedloop(prob) * static_gain_compensation(prob) -@test isstable(Gref) -@test dcgain(Gref) ≈ I(ny) -plot(step(Gref, 100)) +w = exp10.(LinRange(-3, log10(pi / Ts), 200)) +gangoffourplot(prob, w, lab = "", legend = :bottomright) ``` -`G_PS(prob)*Cff` has the same reference transfer function but may contain canceling -controller modes in its realization. `Gref` avoids these redundant modes. +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: + +```@example LQG_DIST +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`](@ref) and [`comp_sensitivity`](@ref). To obtain the corresponding controller, call [`observer_controller`](@ref) for the pure feedback controller, or [`extended_controller`](@ref) for a 2-DOF controller that takes a separate state reference. ## Explicit error integration via LQI diff --git a/src/lqg.jl b/src/lqg.jl index 381568db..d572c6c8 100644 --- a/src/lqg.jl +++ b/src/lqg.jl @@ -154,12 +154,12 @@ function LQGProblem( SQ === nothing && (SQ = zeros(size(sys.B2))) SR === nothing && (SR = zeros(size(sys.C2'))) - d, E = _disturbance_feedback(sys, disturbance_indices, disturbance_input_indices) + d, E = _disturbance_compensation(sys, disturbance_indices, disturbance_input_indices) isempty(d) || iszero(SQ[d, :]) || throw(ArgumentError("SQ must be zero on disturbance variables; their compensation is specified by the disturbance mapping")) LQGProblem(sys, Q1, Q2, R1, R2, qQ, qR, SQ, SR, d, E) end -function _disturbance_feedback(sys, disturbance_indices, input_indices) +function _disturbance_compensation(sys, disturbance_indices, input_indices) d = collect(Int, disturbance_indices) length(unique(d)) == length(d) || throw(ArgumentError("disturbance_indices must be unique")) all(i -> 1 <= i <= sys.nx, d) || throw(ArgumentError("disturbance_indices must index the augmented state vector"))