Clean up extended_controller: canonical 2-DOF, direct kwarg, validation

docs/src/lqg_disturbance.md

@@ -150,3 +150,35 @@ 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`](@ref)
+
+The reference-signal response of an LQG controller can be improved without adding integral action by proper reference feedforward. [`extended_controller`](@ref) returns an [`ExtendedStateSpace`](@ref) 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):
+
+```@example LQG_DIST
+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])
+```
+
+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:
+
+```@example LQG_DIST
+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.

src/lqg.jl

@@ -253,28 +253,10 @@ end
 
 
 """
-    extended_controller(K::AbstractStateSpace)
+    extended_controller(P::StateSpace, L, K; z = nothing, direct = false)
+    extended_controller(l::LQGProblem, L = lqr(l), K = kalman(l); z = nothing, direct = false)
 
-Takes a controller and returns an `ExtendedStateSpace` version which has augmented input `[r; y]` and output `y` (`z` output is 0-dim).
-"""
-function extended_controller(K::AbstractStateSpace)
-    nx,nu,ny = K.nx, K.nu, K.ny
-    A,B,C,D = ssdata(K)
-    @error("This has not been verified")
-
-    B1 = zeros(nx, nx) # dynamics not affected by r
-    B2 = B # input y
-    D21 = C#   K*r
-    C2 = -C # - K*y
-    C1 = zeros(0, nx)
-    ss(A, B1, B2, C1, C2; D21, Ts = K.timeevol)
-end
-
-"""
-    extended_controller(P::StateSpace, L, K; z = nothing)
-    extended_controller(l::LQGProblem, L = lqr(l), K = kalman(l); z = nothing)
-
-Returns a statespace system representing the controller that is obtained when state-feedback `u = L(xᵣ-x̂)` is combined with a Kalman filter with gain `K` that produces state estimates x̂. The controller is an instance of `ExtendedStateSpace` where `C2 = -L, D21 = L` and `B2 = K`.
+Returns a statespace system representing the controller that is obtained when state-feedback `u = L(xᵣ-x̂)` is combined with a Kalman filter with gain `K` that produces state estimates x̂. The controller is an instance of `ExtendedStateSpace` where `C2 = -L, D21 = L` and `B2 = K`. The reference `xᵣ` also enters the observer dynamics through `B1 = (B − KD)·L`, so the observer estimate `x̂` tracks the true state even when `xᵣ ≠ 0`.
 
 The returned system has *inputs* `[xᵣ; y]` and outputs the control signal `u`. If a reference model `R` is used to generate state references `xᵣ`, the controller from `(ry, y) -> u`  where `ry - y = e` is given by
 ```julia
@@ -290,31 +272,52 @@ Ce = extended_controller(l)
 system_mapping(Ce) == -C
 ```
 
-Please note, without the reference pre-filter, the DC gain from references to controlled outputs may not be identity. If a vector of output indices is provided through the keyword argument `z`, the closed-loop system from state reference `xᵣ` to outputs `z` is returned as a second return argument. The inverse of the DC-gain of this closed-loop system may be useful to compensate for the DC-gain of the controller.
+If a vector of output indices is provided through the keyword argument `z`, the closed-loop system from state reference `xᵣ` to outputs `z` is returned as a second return argument. The inverse of the DC-gain of this closed-loop system may be useful as an additional reference pre-filter for plants where exact tracking is desired despite mismatch.
+
+The `direct` keyword argument mirrors the corresponding option on [`observer_controller`](@ref) and, for discrete plants, selects the current-time observer correction. It has no effect on continuous-time plants. When `direct = true`, `K` must be the corresponding "direct" Kalman gain (from `kalman(l; direct = true)`) and `D` must be zero. In direct mode the reference enters as a pure feedforward (`B1 = 0`); the canonical 2-DOF form `B1 = (B − KD)·L` is only used in the non-direct branch.
 """
-function extended_controller(P::AbstractStateSpace, L::AbstractMatrix, K::AbstractMatrix; z::Union{Nothing, AbstractVector} = nothing)
+function extended_controller(P::AbstractStateSpace, L::AbstractMatrix, K::AbstractMatrix; z::Union{Nothing, AbstractVector} = nothing, direct::Bool = false)
     A,B,C,D = ssdata(P)
-    Ac = A - B*L - K*C + K*D*L # 8.26b
     (; nx, nu, ny) = P
-    B1 = zeros(nx, nx) # dynamics not affected by r
-    # l.D21 does not appear here, see comment in kalman
-    B2 = K # input y
-    D21 = L #   L*xᵣ # should be D21?
-    C2 = -L # - L*x̂
-    C1 = zeros(0, nx)
-    Ce0 = ss(Ac, B1, B2, C1, C2; D21, Ts = P.timeevol)
+    size(L) == (nu, nx) || throw(ArgumentError("L must have size (nu, nx) = ($(nu), $(nx)), got $(size(L))"))
+    size(K) == (nx, ny) || throw(ArgumentError("K must have size (nx, ny) = ($(nx), $(ny)), got $(size(K))"))
+    if direct
+        isdiscrete(P) || throw(ArgumentError("direct = true is only meaningful for discrete-time plants"))
+        iszero(D) || throw(ArgumentError("D must be zero when using direct = true (matches observer_controller)"))
+        # Mirror the matrices used by observer_controller(...; direct=true): the y-channel block
+        # (Ac, B2, C2, D22) matches observer_controller exactly up to the sign on (C2, D22), so
+        # `system_mapping(Ce) == -observer_controller(l; direct=true)` holds.
+        IKC = I - K*C
+        ABL = A - B*L
+        Ac  = IKC * ABL
+        B2  = IKC * ABL * K
+        # Reference path: feedforward only — see docstring caveat about non-canonical 2-DOF here.
+        B1  = zeros(nx, nx)
+        # D22 needs to absorb the (-L*K) feedthrough that observer_controller (direct) carries on Dc.
+        D22 = -L * K
+    else
+        Ac  = A - B*L - K*C + K*D*L # 8.26b
+        B1  = (B - K*D) * L         # canonical 2-DOF: observer sees the actual u = L(xᵣ − x̂)
+        B2  = K
+        D22 = zeros(nu, ny)
+    end
+    D21 = L  # L * xᵣ
+    C2  = -L  # − L * x̂
+    C1  = zeros(0, nx)
+    Ce0 = ss(Ac, B1, B2, C1, C2; D21, D22, Ts = P.timeevol)
     if z === nothing
         return Ce0
     end
-    r = 1:nx
+    # `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].
     Ce = ss(Ce0)
-    cl = feedback(P, Ce, Z1 = z, Z2=[], U2=(1:ny) .+ nx, Y1 = :, W2=r, W1=[])
+    cl = feedback(P, Ce, Z1 = z, Z2=[], U2=(1:ny) .+ nx, Y1 = :, W2=1:nx, W1=[])
     Ce0, cl
 end
 
 
 function extended_controller(l::LQGProblem, L::AbstractMatrix = lqr(l), K::AbstractMatrix = kalman(l); kwargs...)
-    P = system_mapping(l, identity)
+    P = system_mapping(l, identity) # identity skips the optional plant transform
     extended_controller(P, L, K; kwargs...)
 end
 

test/test_lqg.jl

@@ -503,8 +503,10 @@ Cfb = observer_controller(lqg)
 cl = feedback(system_mapping(lqg), observer_controller(lqg))*RobustAndOptimalControl.ff_controller(lqg, comp_dc = true)
 @test dcgain(cl)[1,2] ≈ 1 rtol=1e-8
 
-# Method 4: Build compensation into R and compute the closed-loop DC gain, should be 1
-R = named_ss(ss(dc_gain_compensation*I(4)), "R") # Reference filter
+# Method 4: With the canonical 2-DOF form of extended_controller, no R pre-filter is needed —
+# the reference enters the observer dynamics so x̂ tracks x and (for this integrator plant) the
+# closed-loop DC gain from state reference to controlled output is identity.
+R = named_ss(ss(I(4)), "R") # identity pre-filter
 Ce = named_ss(ss(Ce); x = :xC, y = :u, u = [R.y; :y^lqg.ny])
 
 Cry = RobustAndOptimalControl.connect([R, Ce]; u1 = R.y, y1 = R.y, w1 = [R.u; :y^lqg.ny], z1=[:u])
@@ -514,26 +516,61 @@ connections = [
     [:x, :phi] .=> [:y1, :y2]
 ]
 cl = RobustAndOptimalControl.connect([lsys, Cry], connections; w1 = R.u, z1 = [:x, :phi])
-@test inv(dcgain(cl)[1,2]) ≈ 1 rtol=1e-8
+@test dcgain(cl)[1,2] ≈ 1 rtol=1e-8
 
 
 # Method 5: close the loop manually with reference as input and position as output
 
 R = named_ss(ss(I(4)), "R") # Reference filter, used for signal names only
 Ce = named_ss(ss(extended_controller(lqg)); x = :xC, y = :u, u = [:Ry^4; :y^lqg.ny])
 cl = feedback(lsys, Ce, z1 = [:x], z2=[], u2=:y^2, y1 = [:x, :phi], w2=[:Ry2], w1=[])
-@test inv(dcgain(cl)[]) ≈ dc_gain_compensation rtol=1e-8
+@test dcgain(cl)[] ≈ 1 rtol=1e-8
 
 cl = feedback(lsys, Ce, z1 = [:x, :phi], z2=[], u2=:y^2, y1 = [:x, :phi], w2=[:Ry2], w1=[])
-@test pinv(dcgain(cl)) ≈ [dc_gain_compensation 0] atol=1e-8
+@test pinv(dcgain(cl)) ≈ [1 0] atol=1e-8
 
 # Method 6: use the z argument to extended_controller to compute the closed-loop TF
 
 Ce, cl = extended_controller(lqg, z=[1, 2])
-@test pinv(dcgain(cl)[1,2]) ≈ dc_gain_compensation atol=1e-8
+@test dcgain(cl)[1,2] ≈ 1 atol=1e-8
 
 Ce, cl = extended_controller(lqg, z=[1])
-@test pinv(dcgain(cl)[1,2]) ≈ dc_gain_compensation atol=1e-8
+@test dcgain(cl)[1,2] ≈ 1 atol=1e-8
+
+
+# Method 7: discrete-time round-trip — system_mapping(Ce) == -observer_controller(l_d)
+let Ts = 0.05
+    Pd = c2d(P, Ts)
+    lqg_d = LQGProblem(Pd, Q1, Q2, R1, R2)
+    Ce_d = extended_controller(lqg_d)
+    Cfb_d = observer_controller(lqg_d)
+    @test system_mapping(Ce_d) ≈ -ss(Cfb_d)
+end
+
+
+# Method 8: direct=true on a discrete plant — system_mapping(Ce) == -observer_controller(l; direct=true).
+# `observer_controller(::LQGProblem, ::AbstractMatrix, ::AbstractMatrix; direct)` is ambiguous with
+# the ControlSystemsBase fallback when K is concrete, so we let observer_controller resolve K itself.
+let Ts = 0.05
+    Pd = c2d(P, Ts)
+    lqg_d = LQGProblem(Pd, Q1, Q2, R1, R2)
+    Ld = lqr(lqg_d)
+    Kd_direct = kalman(lqg_d; direct=true)
+    Ce_dir = extended_controller(lqg_d, Ld, Kd_direct; direct=true)
+    Cfb_dir = observer_controller(lqg_d; direct=true)
+    @test system_mapping(Ce_dir) ≈ -ss(Cfb_dir)
+end
+
+
+# Argument validation: wrong-sized L or K should ArgumentError
+let
+    P2 = ss(P)
+    L_ok = lqr(lqg)
+    K_ok = kalman(lqg)
+    @test_throws ArgumentError extended_controller(P2, L_ok[:, 1:end-1], K_ok)
+    @test_throws ArgumentError extended_controller(P2, L_ok, K_ok[1:end-1, :])
+end
+
 
 ## Test LQGProblem with NamedStateSpace inside ExtendedStateSpace
 # This tests that the index-based ExtendedStateSpace preserves the type of the internal system