Fix lqi_controller partial/discrete handling; add LQGProblem method; docs port

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`.

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

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