add integrator option to extended_controller

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)
+```

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
 

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)