From c15039a8d23a7f4c53345a489fe3c07234165697 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Mon, 28 Oct 2024 11:57:45 +0100 Subject: [PATCH] add integrator option to `extended_controller` --- docs/src/lqg_disturbance.md | 28 ++++++++++++++++++ src/lqg.jl | 58 ++++++++++++++++++++++++++++--------- test/test_lqg.jl | 18 ++++++++++-- 3 files changed, 89 insertions(+), 15 deletions(-) diff --git a/docs/src/lqg_disturbance.md b/docs/src/lqg_disturbance.md index 15ae7474..811fb9bf 100644 --- a/docs/src/lqg_disturbance.md +++ b/docs/src/lqg_disturbance.md @@ -99,3 +99,31 @@ 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.5, 1.31) to (1.25, 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 have simulated the closed-loop response by creating closed-loop functions directly using [`G_PS`](@ref) and [`comp_sensitivity`](@ref). To extract the controller used underneath the hood in these examples, we may call [`observer_controller`](@ref) or [`extended_controller`](@ref), depending on whether we want a feedback controller only or a 2 DOF controller that takes separate reference inputs. + + +## Explicit error integration +Integral action can also be added to an LQG controller by calling [`extended_controller`](@ref) with the option `output_ref = true` and by providing an integrator gain matrix `Li`. We demonstrate this below + +```@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 + +R1 = 0.001I(nx) # State noise covariance +R2 = I(ny) # measurement noise covariance +prob = LQGProblem(G, Q1, Q2, R1, R2) + +Li = [3;;] # Integral gain +C = extended_controller(prob; output_ref = true, Li) +Gcl = feedback(G, -ss(C), Z1 = [1], Z2=[1], U2=(1:ny) .+ ny, Y1 = :, W2=[], W1=[1], pos_feedback=true) + +res = lsim(Gcl, disturbance, 100) +plot(res, ylabel=["y" "u"]); ylims!((-0.05, 0.3), sp = 1) +``` \ No newline at end of file diff --git a/src/lqg.jl b/src/lqg.jl index b1533e2e..fc0fcdfc 100644 --- a/src/lqg.jl +++ b/src/lqg.jl @@ -271,7 +271,7 @@ function extended_controller(K::AbstractStateSpace) end """ - extended_controller(l::LQGProblem, L = lqr(l), K = kalman(l); z = nothing) + extended_controller(l::LQGProblem, L = lqr(l), K = kalman(l); z = nothing, output_ref = false, Li = 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`. @@ -290,25 +290,57 @@ 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 the option `output_fer = true` is set, the function returns a controller that expects output references `yᵣ` instead of state references `xᵣ`. In this case, `ny` integrators are added that integrate the error \$e = yᵣ - y\$. The integrator gain matrix `Li` of size `(nu, ny)` must be provided in this case. This option is sometimes referred to as "Tracking LQG" or `lqgtrack`. """ -function extended_controller(l::LQGProblem, L::AbstractMatrix = lqr(l), K::AbstractMatrix = kalman(l); z::Union{Nothing, AbstractVector} = nothing) - P = system_mapping(l) - A,B,C,D = ssdata(P) - Ac = A - B*L - K*C + K*D*L # 8.26b +function extended_controller(l::LQGProblem, L::AbstractMatrix = lqr(l), K::AbstractMatrix = kalman(l); z::Union{Nothing, AbstractVector} = nothing, output_ref = false, Li = nothing) + P = system_mapping(l, identity) (; 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) + A,B,C,D = ssdata(P) + if output_ref + Li === nothing && throw(ArgumentError("The integrator gain matrix Li must be provided when output_ref is true")) + size(Li) == (nu, ny) || throw(ArgumentError("The integrator gain matrix Li must have size (nu, ny)")) + # A,B,C,D = l.sys.A, l.sys.B2, l.sys.C1, l.sys.D12 + + if iscontinuous(P) + Ac = [ + (A - B*L - K*C + K*D*L) (K*D*Li-B*Li) + zeros(ny, nx+ny) + ] + else + Ac = [ + (A - B*L - K*C + K*D*L) (K*D*Li-B*Li) + zeros(ny, nx) P.Ts*I(ny) + ] + end + B1 = [ + zeros(nx, ny) + I(ny) + ] + B2 = [ + K + -I(ny) + ] + C1 = zeros(0, nx+ny) + C2 = [-L -Li] + D21 = 0 + + else + # State references + Ac = A - B*L - K*C + K*D*L # 8.26b + 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ᵣ + C1 = zeros(0, nx) + C2 = -L # - L*x̂ + end Ce0 = ss(Ac, B1, B2, C1, C2; D21, Ts = l.timeevol) if z === nothing return Ce0 end - r = 1:nx + r = 1:(output_ref ? ny : nx) 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) .+ (output_ref ? ny : nx), Y1 = :, W2=r, W1=[], pos_feedback=true) Ce0, cl end diff --git a/test/test_lqg.jl b/test/test_lqg.jl index 9d5903cf..560e9b07 100644 --- a/test/test_lqg.jl +++ b/test/test_lqg.jl @@ -502,6 +502,7 @@ Cfb = observer_controller(lqg) # Method 3, using the observer controller and the ff_controller cl = feedback(system_mapping(lqg), observer_controller(lqg))*RobustAndOptimalControl.ff_controller(lqg, comp_dc = true) @test dcgain(cl)[1,2] ≈ 1 rtol=1e-8 +@test isstable(cl) # 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 @@ -515,23 +516,36 @@ connections = [ ] cl = RobustAndOptimalControl.connect([lsys, Cry], connections; w1 = R.u, z1 = [:x, :phi]) @test inv(dcgain(cl)[1,2]) ≈ 1 rtol=1e-8 +@test isstable(cl) # 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=[]) +cl = feedback(lsys, Ce, z1 = [:x], z2=[], u2=:y^2, y1 = [:x, :phi], w2=[:Ry2], w1=[], pos_feedback=true) @test inv(dcgain(cl)[]) ≈ dc_gain_compensation rtol=1e-8 +@test isstable(cl) -cl = feedback(lsys, Ce, z1 = [:x, :phi], z2=[], u2=:y^2, y1 = [:x, :phi], w2=[:Ry2], w1=[]) +cl = feedback(lsys, Ce, z1 = [:x, :phi], z2=[], u2=:y^2, y1 = [:x, :phi], w2=[:Ry2], w1=[], pos_feedback=true) @test pinv(dcgain(cl)) ≈ [dc_gain_compensation 0] atol=1e-8 +@test isstable(cl) # 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 isstable(cl) Ce, cl = extended_controller(lqg, z=[1]) @test pinv(dcgain(cl)[1,2]) ≈ dc_gain_compensation atol=1e-8 +@test isstable(cl) + +## Output references +Li = [1 0] +Cry2, cl = extended_controller(lqg; output_ref = true, Li, z=[1]) +cl = minreal(cl) + +@test dcgain(cl) ≈ [1 0] atol=1e-12 +@test isstable(cl)