add support for computing closed loop in extended_controller

Author: baggepinnen

src/lqg.jl

@@ -68,6 +68,11 @@ Several functions are defined for instances of `LQGProblem`
 - [`observer_controller`](@ref)
 
 A video tutorial on how to use the LQG interface is available [here](https://youtu.be/NuAxN1mGCPs)
+
+## Introduction of references
+The most principled way of introducing references is to add references as measured inputs to the extended statespace model, and to let the performance output `z` be the differences between the references and the outputs for which references are provided. 
+
+A less cumbersome way is not not consider references when constructing the `LQGProblem`, and instead pass the `z` keyword arugment to [`extended_controller`](@ref) in order to obtain a closed-loop system from state references to controlled outputs, and use some form of inverse of the DC gain of this system (or one of its subsystems) to pre-compensate the reference input.
 """
 struct LQGProblem
     sys::ExtendedStateSpace
@@ -266,9 +271,9 @@ function extended_controller(K::AbstractStateSpace)
 end
 
 """
-    extended_controller(l::LQGProblem, L = lqr(l), K = kalman(l))
+    extended_controller(l::LQGProblem, L = lqr(l), K = kalman(l); z = nothing)
 
-Returns an expression for 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 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
@@ -284,22 +289,30 @@ 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.
+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.
 """
-function extended_controller(l::LQGProblem, L = lqr(l), K = kalman(l))
+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
-    nx = l.nx
+    (; 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)
-    ss(Ac, B1, B2, C1, C2; D21, Ts = l.timeevol)
+    Ce0 = ss(Ac, B1, B2, C1, C2; D21, Ts = l.timeevol)
+    if z === nothing
+        return Ce0
+    end
+    r = 1:nx
+    Ce = ss(Ce0)
+    cl = feedback(P, Ce, Z1 = z, Z2=[], U2=(1:ny) .+ nx, Y1 = :, W2=r, W1=[])
+    Ce0, cl
 end
 
+
 """
     observer_controller(l::LQGProblem, L = lqr(l), K = kalman(l))
 
@@ -342,6 +355,16 @@ end
 Return the feedforward controller ``C_{ff}`` that maps references to plant inputs:
 ``u = C_{fb}y + C_{ff}r``
 
+The following should hold
+```
+Cff = RobustAndOptimalControl.ff_controller(l)
+Cfb = observer_controller(l)
+Gcl = feedback(system_mapping(l), Cfb) * Cff # Note the comma in feedback, P/(I + PC) * Cff
+dcgain(Gcl) ≈ I # Or some I-like non-square matrix 
+```
+
+Note, if [`extended_controller`](@ref) is used, the DC-gain compensation above cannot be used. The [`extended_controller`](@ref) assumes that the references enter like `u = L(xᵣ - x̂)`. 
+
 See also [`observer_controller`](@ref).
 """
 function ff_controller(l::LQGProblem, L = lqr(l), K = kalman(l); comp_dc = true)

test/test_extendedstatespace.jl

@@ -1,6 +1,7 @@
 using RobustAndOptimalControl
 using ControlSystemsBase
 using ControlSystemsBase: balance_statespace
+using Test
 
 G = ssrand(3,4,5)
 

test/test_lqg.jl

@@ -1,4 +1,4 @@
-using RobustAndOptimalControl, ControlSystemsBase, LinearAlgebra
+using RobustAndOptimalControl, ControlSystemsBase, LinearAlgebra, Test
 # NOTE: Glad, Ljung chap 9 contains several numerical examples that can be used as test cases
 
 
@@ -402,4 +402,136 @@ predicted_costf = dot([x0; zeros(nu)], QNf, [x0; zeros(nu)])
 @test predicted_costf ≈ predicted_cost
 
 # @show (actual_cost - predicted_cost) / actual_cost
-@test actual_cost ≈ predicted_cost rtol=1e-10
+@test actual_cost ≈ predicted_cost rtol=1e-10
+
+## Double mass model with output penalty
+
+# One input, to z outputs
+P = DemoSystems.double_mass_model(outputs = 1:4)
+# Pe = partition(P, y = 1:2, u = 1)
+Pe = ExtendedStateSpace(P, C1 = P.C[3:4, :], C2 = P.C[1:1, :], B1 = P.C[[2,4], :]')
+
+Q1 = 1.0I(Pe.nz)
+Q2 = 0.1I(Pe.nu)
+R1 = 1.0I(Pe.nw)
+R2 = 0.1I(Pe.ny)
+
+G = LQGProblem(Pe, Q1, Q2, R1, R2)
+Cfb = observer_controller(G)
+Ce = extended_controller(G)
+
+@test Cfb.nu == Pe.ny
+@test Cfb.ny == Pe.nu
+
+
+Cff = RobustAndOptimalControl.ff_controller(G) # TODO: need to add argument that determins whether to use state references or references of z
+@test Cff.nu == Pe.nz
+@test Cff.ny == Pe.nu
+
+Gcl2 = feedback(system_mapping(Pe), Cfb) * Cff
+@test size(Gcl2) == (1, 2)
+@test isstable(Gcl2)
+@test dcgain(Gcl2) ≈ [1.0 0] atol=1e-6
+
+
+
+## One input, one z output
+P = DemoSystems.double_mass_model(outputs = 1:4)
+# Pe = partition(P, y = 1:2, u = 1)
+Pe = ExtendedStateSpace(P, C1 = P.C[3:3, :], C2 = P.C[1:1, :], B1 = P.C[[2,4], :]')
+
+Q1 = 1.0I(Pe.nz)
+Q2 = 0.1I(Pe.nu)
+R1 = 1.0I(Pe.nw)
+R2 = 0.1I(Pe.ny)
+
+G = LQGProblem(Pe, Q1, Q2, R1, R2)
+Cfb = observer_controller(G)
+Ce = extended_controller(G)
+
+@test Cfb.nu == Pe.ny
+@test Cfb.ny == Pe.nu
+
+
+Cff = RobustAndOptimalControl.ff_controller(G)
+@test Cff.nu == Pe.nz
+@test Cff.ny == Pe.nu
+
+Gcl = feedback(system_mapping(Pe) * Cfb)
+@test size(Gcl) == (1, 1)
+@test isstable(Gcl)
+@test dcgain(Gcl)[] ≈ 1.0
+
+Gcl2 = feedback(system_mapping(Pe), Cfb) * Cff
+@test size(Gcl2) == (1, 1)
+@test isstable(Gcl2)
+@test dcgain(Gcl2)[] ≈ 1.0
+
+## Multibody cartpole tests
+
+lsys = let
+    lsysA = [0.0 0.0 0.0 1.0; 0.0 0.0 1.0 0.0; 4.474102070258828 0.0 0.0 0.0; 39.683507165892394 0.0 0.0 0.0]
+    lsysB = [0.0; 0.0; 0.8415814526118872; 2.3385955754360115;;]
+    lsysC = [0.0 1.0 0.0 0.0; 1.0 0.0 0.0 0.0; 0.0 0.0 1.0 0.0; 0.0 0.0 0.0 1.0]
+    lsysD = [0.0; 0.0; 0.0; 0.0;;]
+    named_ss(ss(lsysA, lsysB, lsysC, lsysD), x=[:revolute₊phi, :prismatic₊s, :prismatic₊v, :revolute₊w], u=[Symbol("u(t)")], y=[Symbol("x(t)"), Symbol("phi(t)"), Symbol("v(t)"), Symbol("w(t)")])
+end
+
+C = lsys.C
+Q1 = Diagonal([10, 10, 10, 1])
+Q2 = Diagonal([0.1])
+
+R1 = lsys.B*Diagonal([1])*lsys.B'
+R2 = Diagonal([0.01, 0.01])
+Pn = lsys[[:x, :phi], :]
+P = ss(Pn)
+
+lqg = LQGProblem(P, Q1, Q2, R1, R2)
+
+# Below we test that the closed-loop DC gain from references to cart position is 1
+Ce = extended_controller(lqg)
+
+
+# Method 1: compute DC gain compensation using ff_controller, this is used as reference for the others. I feel uneasy about the (undocumented) comp_dc = false argument here, ideally a more generally applicable function ff_controller would be implemented that can handle both state and output references etc.
+dc_gain_compensation = dcgain(RobustAndOptimalControl.ff_controller(lqg, comp_dc = false))[]
+
+# Method 2, using the observer controller and static_gain_compensation
+Cfb = observer_controller(lqg)
+@test inv(dcgain((feedback(P, Cfb)*static_gain_compensation(lqg))[1,2]))[] ≈ dc_gain_compensation rtol=1e-8
+
+# 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
+
+# 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
+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])
+
+connections = [
+    :u => :u
+    [: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
+
+
+# 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
+
+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
+
+# 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
+
+Ce, cl = extended_controller(lqg, z=[1])
+@test pinv(dcgain(cl)[1,2]) ≈ dc_gain_compensation atol=1e-8
+