diff --git a/docs/src/lqg_disturbance.md b/docs/src/lqg_disturbance.md index e19c18be..bc176a6d 100644 --- a/docs/src/lqg_disturbance.md +++ b/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. diff --git a/src/lqg.jl b/src/lqg.jl index d1bb7b18..532f6aab 100644 --- a/src/lqg.jl +++ b/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 diff --git a/test/test_lqg.jl b/test/test_lqg.jl index 209d2396..d4ea77fa 100644 --- a/test/test_lqg.jl +++ b/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,7 +516,7 @@ 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 @@ -522,18 +524,53 @@ cl = RobustAndOptimalControl.connect([lsys, Cry], connections; w1 = R.u, z1 = [: 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