Merge pull request #105 from JuliaControl/better_lqg

Better lqg

Author: baggepinnen

src/ExtendedStateSpace.jl

@@ -654,7 +654,15 @@ Partition `P` into an [`ExtendedStateSpace`](@ref).
 """
 function partition(P::AbstractStateSpace; u=nothing, y=nothing,
     w = nothing,
-    z = nothing
+    z = nothing,
+    B1 = nothing,
+    B2 = nothing,
+    C1 = nothing,
+    C2 = nothing,
+    D11 = nothing,
+    D12 = nothing,
+    D21 = nothing,
+    D22 = nothing,
 )
     if w === nothing
         w = setdiff(1:P.nu, u)
@@ -672,8 +680,16 @@ function partition(P::AbstractStateSpace; u=nothing, y=nothing,
     y = vcat(y)
     z = vcat(z)
     w = vcat(w)
-    ss(P.A, P.B[:, w], P.B[:, u], P.C[z, :], P.C[y, :], 
-    P.D[z, w], P.D[z, u], P.D[y, w], P.D[y, u], P.timeevol)
+    ss(P.A,
+    B1 === nothing ? P.B[:, w] : B1,
+    B2 === nothing ? P.B[:, u] : B2,
+    C1 === nothing ? P.C[z, :] : C1,
+    C2 === nothing ? P.C[y, :] : C2 ,
+    D11 === nothing ? P.D[z, w] : D11,
+    D12 === nothing ? P.D[z, u] : D12,
+    D21 === nothing ? P.D[y, w] : D21,
+    D22 === nothing ? P.D[y, u] : D22,
+    P.timeevol)
 end
 
 """

src/RobustAndOptimalControl.jl

@@ -28,9 +28,6 @@ using DescriptorSystems: dss
 
 using ChainRulesCore
 
-export show_construction, vec2sys
-include("utils.jl")
-
 export dss, hinfnorm2, linfnorm2, h2norm, hankelnorm, nugap, νgap, baltrunc2, stab_unstab, baltrunc_unstab, baltrunc_coprime
 include("descriptor.jl")
 
@@ -83,4 +80,7 @@ include("mimo_diskmargin.jl")
 export nu_reduction, nu_reduction_recursive
 include("mcm_nugap.jl")
 
+export show_construction, vec2sys
+include("utils.jl")
+
 end

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,15 +271,15 @@ 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 `e = ry - y -> u` is given by
+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
 Ce = extended_controller(l)
-Ce = named_ss(Ce; x = :xC, y = :u, u = [R.y; :y^l.ny]) # Name the inputs of Ce the same as the outputs of `R`.
-connect([R, Ce]; u1 = R.y, y1 = R.y, w1 = [:ry^l.ny, :y^l.ny], z1=[:u])
+Ce = named_ss(ss(Ce); x = :xC, y = :u, u = [R.y; :y^l.ny]) # Name the inputs of Ce the same as the outputs of `R`.
+connect([R, Ce]; u1 = R.y, y1 = R.y, w1 = [R.u; :y^l.ny], z1=[:u])
 ```
 
 Since the negative part of the feedback is built into the returned system, we have
@@ -283,20 +288,31 @@ C = observer_controller(l)
 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.
 """
-function extended_controller(l::LQGProblem, L = lqr(l), K = kalman(l))
-    A,B,C,D = ssdata(system_mapping(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))
 
@@ -339,23 +355,39 @@ 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))
+function ff_controller(l::LQGProblem, L = lqr(l), K = kalman(l); comp_dc = true)
     Ae,Be,Ce,De = ssdata(system_mapping(l, identity))
     Ac = Ae - Be*L - K*Ce + K*De*L # 8.26c
-    Bc = Be*static_gain_compensation(l, L)
     Cc = L
     Dc = 0
-    return 1 - ss(Ac, Bc, Cc, Dc, l.timeevol)
+    if comp_dc
+        Lr = static_gain_compensation(l, L)
+        Bc = Be*Lr
+        return Lr - ss(Ac, Bc, Cc, Dc, l.timeevol)
+    else
+        Bc = Be
+        return I(size(Cc, 1)) - ss(Ac, Bc, Cc, Dc, l.timeevol)
+    end
 end
 
 """
     closedloop(l::LQGProblem, L = lqr(l), K = kalman(l))
 
 Closed-loop system as defined in Glad and Ljung eq. 8.28. Note, this definition of closed loop is not the same as lft(P, K), which has B1 instead of B2 as input matrix. Use `lft(l)` to get the system from disturbances to controlled variables `w -> z`.
 
-The return value will be the closed loop from reference only, other disturbance signals (B1) are ignored. See [`feedback`](@ref) for a more advanced option.
+The return value will be the closed loop from filtred reference only, other disturbance signals (B1) are ignored. See [`feedback`](@ref) for a more advanced option. This function assumes that the control signal is computed as `u = r̃ - Lx̂` (not `u = L(xᵣ - x̂)`), i.e., the feedforward signal `r̃` is added directly to the plant input. `r̃` must thus be produced by an inverse-like model that takes state references and output the feedforward signal.
 
 Use `static_gain_compensation` to adjust the gain from references acting on the input B2, `dcgain(closedloop(l))*static_gain_compensation(l) ≈ I`
 """

src/named_systems2.jl

@@ -401,7 +401,7 @@ function ControlSystemsBase.feedback(s1::NamedStateSpace{T}, s2::NamedStateSpace
     @assert sys.nu == length(W1) + length(W2)
     @assert sys.ny == length(Z1) + length(Z2)
     @assert sys.nx == length(x1)
-    nsys = NamedStateSpace{T,typeof(sys)}(sys, x1, s1.u[[W1; W2]], s1.y[[Z1; Z2]], "")
+    nsys = NamedStateSpace{T,typeof(sys)}(sys, x1, [s1.u[W1]; s2.u[W2]], [s1.y[Z1]; s2.y[Z2]], "")
     sminreal(nsys)
 end
 
@@ -697,30 +697,51 @@ end
 
 function partition(P::NamedStateSpace; u=nothing, y=nothing,
     w = nothing,
-    z = nothing
+    z = nothing,
+    B1 = nothing,
+    B2 = nothing,
+    C1 = nothing,
+    C2 = nothing,
+    D11 = nothing,
+    D12 = nothing,
+    D21 = nothing,
+    D22 = nothing,
 )
     if w === nothing
-        w = names2indices(setdiff(P.u, u), P.u)
-        u = names2indices(u, P.u)
+        inds = names2indices(u, P.u)
+        w = setdiff(1:P.nu, inds)
+        u = inds
     end
     if z === nothing
-        z = names2indices(setdiff(P.y, y), P.y)
-        y = names2indices(y, P.y)
+        inds = names2indices(y, P.y)
+        z = setdiff(1:P.ny, inds)
+        y = inds
     end
     if u === nothing
-        u = names2indices(setdiff(P.u, w), P.u)
-        w = names2indices(w, P.u)
+        inds = names2indices(w, P.u)
+        u = setdiff(1:P.nu, inds)
+        w = inds
     end
     if y === nothing
-        y = names2indices(setdiff(P.y, z), P.y)
-        z = names2indices(z, P.y)
+        inds = names2indices(z, P.y)
+        y = setdiff(1:P.ny, inds)
+        z = inds
     end
     u = vcat(u)
     y = vcat(y)
     z = vcat(z)
     w = vcat(w)
-    ss(P.A, P.B[:, w], P.B[:, u], P.C[z, :], P.C[y, :], 
-    P.D[z, w], P.D[z, u], P.D[y, w], P.D[y, u], P.timeevol)
+    ss(P.A,
+        B1 === nothing ? P.B[:, w] : B1,
+        B2 === nothing ? P.B[:, u] : B2,
+        C1 === nothing ? P.C[z, :] : C1,
+        C2 === nothing ? P.C[y, :] : C2 ,
+        D11 === nothing ? P.D[z, w] : D11,
+        D12 === nothing ? P.D[z, u] : D12,
+        D21 === nothing ? P.D[y, w] : D21,
+        D22 === nothing ? P.D[y, u] : D22,
+        P.timeevol
+    )
 end
 
 function CS.c2d(s::NamedStateSpace{Continuous}, Ts::Real, method::Symbol = :zoh, args...;

src/utils.jl

@@ -48,6 +48,24 @@ function show_construction(io::IO, sys::LTISystem; name = "temp", letb = true)
     nothing
 end
 
+function show_construction(io::IO, sys::NamedStateSpace; name = "temp", letb = true)
+    # sys = StateSpace(sys)
+    letb && println(io, "$name = let")
+    prestr = letb ? "    " : "" 
+    println(io, prestr*"$(name)A = ", sys.A)
+    println(io, prestr*"$(name)B = ", sys.B)
+    println(io, prestr*"$(name)C = ", sys.C)
+    println(io, prestr*"$(name)D = ", sys.D)
+    letb || print(io, "$name = ")
+    if isdiscrete(sys)
+        println(io, prestr*"named_ss(ss($(name)A, $(name)B, $(name)C, $(name)D), $(sys.Ts), x=$(sys.x), u=$(sys.u), y=$(sys.y))")
+    else
+        println(io, prestr*"named_ss(ss($(name)A, $(name)B, $(name)C, $(name)D), x=$(sys.x), u=$(sys.u), y=$(sys.y))")
+    end
+    letb && println(io, "end")
+    nothing
+end
+
 function Base.vec(sys::LTISystem)
     [vec(sys.A); vec(sys.B); vec(sys.C); vec(sys.D)]
 end

test/test_extendedstatespace.jl

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