From ac58c12a51c0d3550004fdb8771b651267cba694 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Thu, 14 May 2026 20:12:42 +0200 Subject: [PATCH] Fix lqi_controller partial outputs, discrete time, and add LQGProblem method MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - lqi_controller silently mis-wired when integrator_outputs != 1:ny because nr was overwritten to ny. Now nr = length(integrator_outputs) and the feedback comparator/integrator/reference symbols all use the correct subset. - ss(lqi(...)) was created without a timeevol, so the gain block was always continuous-time and connect() rejected it for discrete plants. Stamp the plant's timeevol on it. - Replace c2d(1/s, Ts) for the controller integrator with the same form used by add_output_integrator(::Discrete) (Ts/(z-(1-ϵ))) and expose ϵ as a kwarg so plant augmentation and controller agree even with leakage. - Add lqi_controller(prob::LQGProblem, Qi; ...) that builds the Kalman observer internally and forms Q1_aug = blkdiag(prob.Q1, Qi). - lqi docstring: replace stale Q3 signature with args... and clarify that the reference enters in lqi_controller, not in the augmented plant. - New tests cover MIMO continuous, partial integrator_outputs, discrete-time with ϵ, and the LQGProblem method. - Add an LQI section to docs/src/lqg_disturbance.md showing the explicit error-integration design alongside the disturbance-model approach. Co-Authored-By: Claude Opus 4.7 (1M context) --- docs/src/lqg_disturbance.md | 44 ++++++++++++++++++ src/lqg.jl | 92 +++++++++++++++++++++++-------------- test/test_lqi.jl | 70 ++++++++++++++++++++++++++++ 3 files changed, 171 insertions(+), 35 deletions(-) diff --git a/docs/src/lqg_disturbance.md b/docs/src/lqg_disturbance.md index b0a74bb7..f1db1a49 100644 --- a/docs/src/lqg_disturbance.md +++ b/docs/src/lqg_disturbance.md @@ -105,3 +105,47 @@ plot(f1, f2, titlefontsize=10) ``` 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. + +## 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). + +We re-use the original (un-augmented) plant `G` from the top of this page: + +```@example LQG_DIST +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*Matrix{Float64}(I(nx)), 3*Matrix{Float64}(I(ny)); dims=(1, 2)) +Q2 = 0.01*Matrix{Float64}(I(nu)) + +# Observer for the un-augmented plant +R1 = 0.001*Matrix{Float64}(I(nx)) +R2 = Matrix{Float64}(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 +``` + +To inject the load disturbance from earlier (a unit step added at the plant input), we close the loop using only the measurement column of `C` and feed the disturbance into the plant input: + +```@example LQG_DIST +Cy = ss(C)[:, 2] # measurement column (r=0) +Kctrl = -Cy # equivalent negative-feedback controller +Gcl_y = feedback(G, Kctrl) # disturbance → y +Gcl_u = -Kctrl * Gcl_y # disturbance → u +Gcl = [Gcl_y; Gcl_u] +res = lsim(Gcl, disturbance, 100) +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`. diff --git a/src/lqg.jl b/src/lqg.jl index 00f637c4..d1bb7b18 100644 --- a/src/lqg.jl +++ b/src/lqg.jl @@ -637,33 +637,31 @@ Base.@deprecate gangoffour2(P, C) gangoffour(P, C) Base.@deprecate gangoffourplot2(P, C) gangoffourplot(P, C) """ - lqi(sys::AbstractStateSpace, Q1::AbstractMatrix, Q2::AbstractMatrix, Q3::AbstractMatrix; + lqi(sys::AbstractStateSpace, Q1::AbstractMatrix, Q2::AbstractMatrix, args...; integrator_outputs=1:sys.ny, ϵ=0) Calculate the feedback gain for the LQI (Linear-Quadratic-Integral) cost function: ```math x_a^{T} Q_1 x_a + u^{T} Q_2 u ``` -where `x_a = [x; e_i]` is the augmented state vector, `e_i` is the integral of the tracking error for the specified outputs. +where `x_a = [x; x_i]` is the augmented state vector and `x_i` integrates `-Cx` for the selected outputs. -The system is augmented with integrators on the tracking error for the outputs specified in `integrator_outputs`. -The augmented system has the form: +The open-loop plant is augmented via [`add_output_integrator`](@ref) with `neg=true`, giving ```math -\\begin{bmatrix} \\dot{x} \\\\ \\dot{x_i} \\end{bmatrix} = -\\begin{bmatrix} A & 0 \\\\ -C & 0 \\end{bmatrix} -\\begin{bmatrix} x \\\\ x_i \\end{bmatrix} + +\\begin{bmatrix} \\dot{x} \\\\ \\dot{x_i} \\end{bmatrix} = +\\begin{bmatrix} A & 0 \\\\ -C & 0 \\end{bmatrix} +\\begin{bmatrix} x \\\\ x_i \\end{bmatrix} + \\begin{bmatrix} B \\\\ 0 \\end{bmatrix} u ``` - -where the integrator dynamics are `ẋᵢ = r-Cx` (tracking error), ensuring that the controller has integral action -to eliminate steady-state errors. +The reference `r` enters in the closed-loop construction in [`lqi_controller`](@ref) (as `ẋᵢ = r - Cx`), not in the augmented plant produced here. # Arguments: - `sys`: The system to control -- `Q1`: Augmented state cost matrix (must be positive semi-definite) -- `Q2`: Control cost matrix (must be positive definite) -- `integrator_outputs`: Vector of output indices to add integrators for (default: all outputs) -- `ϵ`: Small positive number to move integrator poles slightly into the stable region (default: 0) +- `Q1`: Augmented state cost matrix of size `(nx + length(integrator_outputs))` (must be positive semi-definite) +- `Q2`: Control cost matrix (must be positive definite) +- `args...`: Additional positional arguments forwarded to `lqr`, e.g., an input-state cross-term `S`/`Q3`. +- `integrator_outputs`: Output indices to add integrators for (default: all outputs). Accepts `Int`, `AbstractVector{Int}`, or `AbstractRange`. +- `ϵ`: Move integrator poles slightly into the stable region (default: 0) # Returns: - `L`: The optimal feedback gain matrix for the augmented system @@ -680,7 +678,7 @@ sys = ss(A, B, C, 0) # Design LQI controller Q1 = diagm([1, 1, 10]) # State cost + error-integral cost -Q2 = [1;;] # Control cost +Q2 = [1;;] # Control cost L = lqi(sys, Q1, Q2) ``` @@ -703,13 +701,20 @@ end """ - lqi_controller(G, obs, Q1, Q2) + lqi_controller(G, obs, Q1, Q2, args...; integrator_outputs=1:G.ny, ϵ=0) + lqi_controller(prob::LQGProblem, Qi; integrator_outputs=1:prob.sys.ny, ϵ=0) + +Return an LQI controller with reference and measurement inputs `[r; y]` for the LQI problem in which the plant state `x` is augmented with output-error integrators to form the augmented state `x_a = [x; x_i]`. The number of reference channels equals `length(integrator_outputs)`; non-integrated outputs are fed to the observer but not to the integrator. -Return an LQI controller with reference and measurement inputs `[r; y]` designed for the LQI problem where the plant state `x` is augmented with output-error integrators to form the augmented state `x_a = [x; e_i]`. +# Arguments: +- `Q1`: Penalty on the augmented state of size `(nx + length(integrator_outputs))` (must be positive semi-definite). +- `Q2`: Penalty on the control input (must be positive definite). +- `obs`: An observer for `G` constructed using `observer_predictor(G, K; output_state=true)`. +- `args...`: Additional positional arguments forwarded to `lqr` (e.g., a cross term `S`/`Q3`). +- `integrator_outputs`: Output indices to add integrators for (default: all outputs). +- `ϵ`: Pole offset for the integrator (continuous: `1/(s+ϵ)`; discrete: `Ts/(z-(1-ϵ))`). Matches the form used by [`add_output_integrator`](@ref). -- `Q1`: Penalty matrix for the augmented state (must be positive semi-definite) -- `Q2`: Penalty matrix for the control input (must be positive definite) -- `obs`: An observer for `G` constructed using `observer_predictor(G, ..., output_state=true)` +The `LQGProblem` method builds the Kalman observer internally from `prob`, takes the augmented LQR weights as `Q1_aug = blkdiag(prob.Q1, Qi)`, and uses `prob.Q2` as the control weight. Example: ``` @@ -732,26 +737,33 @@ plot( ) ``` """ -function lqi_controller(G, obs, Q1, Q2, args...) - L = named_ss(ss(lqi(G, Q1, Q2, args...)), name="L", x=:x_L, y=:y_L, u=:u_L) +function lqi_controller(G, obs, Q1, Q2, args...; integrator_outputs=1:G.ny, ϵ=0) + L_mat = lqi(G, Q1, Q2, args...; integrator_outputs, ϵ) + L_ss = iscontinuous(G) ? ss(L_mat) : ss(L_mat, G.Ts) + L = named_ss(L_ss, name="L", x=:x_L, y=:y_L, u=:u_L) G isa NamedStateSpace || (G = named_ss(G, name="plant", x=:x_plant, y=:y_plant, u=:u_plant)) obs isa NamedStateSpace || (obs = named_ss(obs, name="observer", x=:x_observer, y=:y_observer, u=:u_observer)) - - s = tf('s') + (; nx, nu, ny) = G - nr = size(L, 2) - nx + nr = length(integrator_outputs) te = G.timeevol - nr = ny aug_state_inds = 1:nx - aug_integrator_inds = nx+1:size(L, 2) - - refs = Symbol.(string.(G.y) .* "_r") - feedback_y = Symbol.(string.(G.y) .* "_fb") + aug_integrator_inds = nx+1:nx+nr + + refs = Symbol.(string.(G.y[integrator_outputs]) .* "_r") + feedback_y = Symbol.(string.(G.y[integrator_outputs]) .* "_fb") add_feedback = named_ss(ss([I(nr) -I(nr)], te), u=[refs; feedback_y], y=:e) - int0 = iscontinuous(G) ? ss(1/s) : ss(c2d(1/s, G.Ts)) - integrator = named_ss(I(nr) .* int0, u=:e, y=:ie, x=:x_int) - unit_gain = named_ss(ss(I(nr), te), u=G.y) # The plant output is not available as input in any of the components, so we introduce this unit gain to introduce a component with a plant-input input. This is then fed into multiple places + # Match the integrator form used by `add_output_integrator` so plant augmentation and controller agree + if iscontinuous(G) + s = tf('s') + int_scalar = ss(1/(s+ϵ)) + else + int_scalar = ss(tf(G.Ts, [1, -(1-ϵ)], G.Ts)) + end + integrator = named_ss(I(nr) .* int_scalar, u=:e, y=:ie, x=:x_int) + # unit_gain fans the full y vector out to the observer (all ny channels) and to the feedback comparator (only the integrated subset) + unit_gain = named_ss(ss(I(ny), te), u=G.y) external_inputs = [ refs; G.y; @@ -769,8 +781,18 @@ function lqi_controller(G, obs, Q1, Q2, args...) obs.y .=> L.u[aug_state_inds]; L.y .=> obs.u[observer_input_inds]; unit_gain.y .=> obs.u[observer_output_inds]; - unit_gain.y .=> add_feedback.u[nr+1:end] + unit_gain.y[integrator_outputs] .=> add_feedback.u[nr+1:end] ] - # Negate obs to output -x̂ + # Negate obs to output -x̂, so L*(-x̂) = -L*x̂ connect([add_feedback, integrator, L, -obs, unit_gain], connections; external_inputs, external_outputs) end + +function lqi_controller(prob::LQGProblem, Qi::AbstractMatrix; integrator_outputs=1:prob.sys.ny, ϵ=0) + G = system_mapping(prob, identity) + K = kalman(prob) + obs = observer_predictor(G, K; output_state=true) + size(Qi, 1) == size(Qi, 2) == length(integrator_outputs) || + throw(ArgumentError("Qi must be square with size length(integrator_outputs) = $(length(integrator_outputs))")) + Q1_aug = cat(prob.Q1, Qi; dims=(1, 2)) + lqi_controller(G, obs, Q1_aug, prob.Q2; integrator_outputs, ϵ) +end diff --git a/test/test_lqi.jl b/test/test_lqi.jl index 0b422884..a0f8b7a9 100644 --- a/test/test_lqi.jl +++ b/test/test_lqi.jl @@ -28,3 +28,73 @@ H = feedback(C0, Gn, w1 = :y_plant_r, z2=Gn.y, u1=:y_plant, pos_feedback=true) res = step(H, 50) @test res.y[:, end] ≈ [dcgain(feedback(-C0[:, 2], G))[]; 1.0] # plot(res) + + +# MIMO continuous, all outputs integrated +G2 = ss([-1.0 0.2; 0.1 -2.0], [1.0 0; 0 1.0], [1.0 0; 0 1.0], 0) +K2 = kalman(G2, I(G2.nx), I(G2.ny)) +obs2 = observer_predictor(G2, K2; output_state=true) +Q1_mimo = diagm([1.0, 1.0, 10.0, 10.0]) +Q2_mimo = Matrix{Float64}(I(G2.nu)) +C_mimo = RobustAndOptimalControl.lqi_controller(G2, obs2, Q1_mimo, Q2_mimo) +@test C_mimo.nu == 2*G2.ny # [r; y] +@test C_mimo.ny == G2.nu +# Each output should have an integral mode in the controller +@test count(p -> abs(p) < 1e-6, poles(C_mimo)) == G2.ny + +ref_syms_mimo = Symbol.("y_plant" .* string.(1:G2.ny) .* "_r") +y_plant_syms = Symbol.("y_plant" .* string.(1:G2.ny)) +G2n = named_ss(G2, name="plant", x=:x_plant, y=:y_plant, u=:u_plant) +H2 = feedback(C_mimo, G2n, w1 = ref_syms_mimo, + z2 = G2n.y, u1 = y_plant_syms, pos_feedback = true) +@test isstable(minreal(H2)) +# Reference-to-output DC gain should be identity on the plant outputs. +# H2 outputs the plant outputs (z2 = G2n.y) and any external outputs of C_mimo (its u). +H2_dc = dcgain(H2) +# Extract the y-subset of outputs by name +y_out_inds = [findfirst(==(s), H2.y) for s in G2n.y] +@test H2_dc[y_out_inds, :] ≈ I(G2.ny) atol=1e-8 + + +# Partial integrator_outputs: integrate only output 1 on a 2-output plant +Q1_partial = diagm([1.0, 1.0, 10.0]) # nx + 1 integrator +C_partial = RobustAndOptimalControl.lqi_controller(G2, obs2, Q1_partial, Q2_mimo; integrator_outputs=[1]) +# Controller takes [r_for_integrated_output; all y] = [1 ref; ny measurements] +@test C_partial.nu == 1 + G2.ny +@test count(p -> abs(p) < 1e-6, poles(C_partial)) == 1 +H_partial = feedback(C_partial, G2n, w1 = [ref_syms_mimo[1]], + z2 = G2n.y, u1 = y_plant_syms, pos_feedback = true) +@test isstable(minreal(H_partial)) +H_partial_dc = dcgain(H_partial) +y_out_inds_p = [findfirst(==(s), H_partial.y) for s in G2n.y] +# Only the integrated output should track exactly +@test H_partial_dc[y_out_inds_p[1], 1] ≈ 1 atol=1e-8 + + +# Discrete SISO with explicit ϵ +Ts = 0.1 +Gd = c2d(G, Ts) +Kd = kalman(Gd, Q, R) +obsd = observer_predictor(Gd, Kd; output_state=true) +Q1d = diagm([0.488, 0, 100.0]) +Q2d = [1/100;;] +ϵd = 1e-4 +Cd = RobustAndOptimalControl.lqi_controller(Gd, obsd, Q1d, Q2d; ϵ=ϵd) +@test Cd.nu == 2 +# Discrete integrator pole near 1 (offset by ϵ) +@test any(p -> abs(p - (1 - ϵd)) < 1e-6, poles(Cd)) +Gdn = named_ss(Gd) +Hd = feedback(Cd, Gdn, w1 = :y_plant_r, z2=Gdn.y, u1=:y_plant, pos_feedback=true) +@test isstable(minreal(Hd)) +@test dcgain(Hd)[2] ≈ 1 atol=1e-2 + + +# LQGProblem method +prob = LQGProblem(G, diagm([0.488, 0]), [1/100;;], Matrix{Float64}(Q), Matrix{Float64}(R)) +Qi = [100.0;;] +C_prob = RobustAndOptimalControl.lqi_controller(prob, Qi) +@test C_prob.nu == 2 +@test 0 ∈ poles(C_prob) +# Dimensions should match the explicit-observer form +@test C_prob.nx == C0.nx +@test C_prob.ny == C0.ny