LQG overhaul (#332)

* use getproperty instead of getfield for LQG

* more updates for LQG

* import getindex

* merge updates from working

* update formatting in docstring

* handle messed-up merge

* fix backslash

Author: baggepinnen

src/ControlSystems.jl

@@ -99,7 +99,7 @@ import Polynomials: Polynomial, coeffs
 using OrdinaryDiffEq, DelayDiffEq
 export Plots
 import Base: +, -, *, /, (==), (!=), isapprox, convert, promote_op
-import Base: getproperty
+import Base: getproperty, getindex
 import Base: exp # for exp(-s)
 import LinearAlgebra: BlasFloat
 export lyap # Make sure LinearAlgebra.lyap is available

src/types/lqg.jl

@@ -2,50 +2,52 @@
 import Base.getindex
 
 """
-    G = LQG(A,B,C,D, Q1, Q2, R1, R2; qQ=0, qR=0, integrator=false)
-    G = LQG(sys, args...; kwargs...)
+    G = LQG(sys::AbstractStateSpace, Q1, Q2, R1, R2; qQ=0, qR=0, integrator=false, M = sys.C)
 
 Return an LQG object that describes the closed control loop around the process `sys=ss(A,B,C,D)`
 where the controller is of LQG-type. The controller is specified by weight matrices `Q1,Q2`
 that penalizes state deviations and control signal variance respectively, and covariance
 matrices `R1,R2` which specify state drift and measurement covariance respectively.
 This constructor calls [`lqr`](@ref) and [`kalman`](@ref) and forms the closed-loop system.
 
-If `integrator=true`, the resulting controller will have intregral action.
+If `integrator=true`, the resulting controller will have integral action.
 This is achieved by adding a model of a constant disturbance on the inputs to the system
-described by `A,B,C,D`.
+described by `sys`.
 
 `qQ` and `qR` can be set to incorporate loop transfer recovery, i.e.,
 ```julia
 L = lqr(A, B, Q1+qQ*C'C, Q2)
 K = kalman(A, C, R1+qR*B*B', R2)
 ```
 
-# Fields
+`M` is a matrix that defines the controlled variables `z`, if none is provided, the default is to consider all measured outputs `y` of the system as controlled. The definitions of `z` and `y` are given below
+```
+y = C*x
+z = M*x
+```
+
+# Fields and properties
 When the LQG-object is populated by the lqg-function, the following fields have been made available
 - `L` is the feedback matrix, such that `A-BL` is stable. Note that the length of the state vector (and the width of L) is increased by the number of inputs if the option `integrator=true`.
 - `K` is the kalman gain such that `A-KC` is stable
 - `sysc` is a dynamical system describing the controller `u=L*inv(A-BL-KC+KDL)Ky`
 
-# Functions
-Several other properties of the object are accessible with the indexing function `getindex()`
-and are called with the syntax `G[:function]`. The available functions are
+Several other properties of the object are accessible as properties. The available properties are
 (some have many alternative names, separated with / )
 
--`G[:cl] / G[:closedloop]` is the closed-loop system, including observer, from reference to output, precompensated to have static gain 1 (`u = −Lx + lᵣr`).
--`G[:S] / G[:Sin]` Input sensitivity function
--`G[:T] / G[:Tin]` Input complementary sensitivity function
--`G[:Sout]` Output sensitivity function
--`G[:Tout]` Output complementary sensitivity function
--`G[:CS]` The transfer function from measurement noise to control signal
--`G[:DS]` The transfer function from input load disturbance to output
--`G[:lt] / G[:looptransfer] / G[:loopgain]  =  PC`
--`G[:rd] / G[:returndifference]  =  I + PC`
--`G[:sr] / G[:stabilityrobustness]  =  I + inv(PC)`
--`G[:sysc] / G[:controller]` Returns the controller as a StateSpace-system
-
-It is also possible to access all fileds using the `G[:symbol]` syntax, the fields are `P
-,Q1,Q2,R1,R2,qQ,qR,sysc,L,K,integrator`
+- `G.cl / G.closedloop` is the closed-loop system, including observer, from reference to output, precompensated to have static gain 1 (`u = −Lx + lᵣr`).
+- `G.S / G.Sin` Input sensitivity function
+- `G.T / G.Tin` Input complementary sensitivity function
+- `G.Sout` Output sensitivity function
+- `G.Tout` Output complementary sensitivity function
+- `G.CS` The transfer function from measurement noise to control signal
+- `G.DS` The transfer function from input load disturbance to output
+- `G.lt / G.looptransfer / G.loopgain  =  PC`
+- `G.rd / G.returndifference  =  I + PC`
+- `G.sr / G.stabilityrobustness  =  I + inv(PC)`
+- `G.sysc / G.controller` Returns the controller as a StateSpace-system
+
+It is also possible to access all fileds using the `G.symbol` syntax, the fields are `P,Q1,Q2,R1,R2,qQ,qR,sysc,L,K,integrator`
 
 # Example
 
@@ -64,9 +66,9 @@ R2 = 1eye(2)
 
 G = LQG(sys, Q1, Q2, R1, R2, qQ=qQ, qR=qR, integrator=true)
 
-Gcl = G[:cl]
-T = G[:T]
-S = G[:S]
+Gcl = G.cl
+T = G.T
+S = G.S
 sigmaplot([S,T],exp10.(range(-3, stop=3, length=1000)))
 stepplot(Gcl)
 ```
@@ -83,148 +85,174 @@ struct LQG
     sysc::LTISystem
     L::AbstractMatrix
     K::AbstractMatrix
+    M::AbstractMatrix
     integrator::Bool
 end
 
 # Provide some constructors
-function LQG(A,B,C,D,Q1::AbstractMatrix,Q2::AbstractMatrix,R1::AbstractMatrix,R2::AbstractMatrix; qQ=0, qR=0, integrator=false)
-    integrator ? _LQGi(A,B,C,D,Q1,Q2,R1,R2, qQ, qR) : _LQG(A,B,C,D,Q1,Q2,R1,R2, qQ, qR)
+function LQG(
+    sys::LTISystem,
+    Q1::AbstractMatrix,
+    Q2::AbstractMatrix,
+    R1::AbstractMatrix,
+    R2::AbstractMatrix;
+    qQ = 0,
+    qR = 0,
+    integrator = false,
+    kwargs...,
+)
+    integrator ? _LQGi(sys, Q1, Q2, R1, R2, qQ, qR; kwargs...) :
+    _LQG(sys, Q1, Q2, R1, R2, qQ, qR; kwargs...)
 end # (1) Dispatches to final
 
-function LQG(A,B,C,D,Q1::AbstractVector,Q2::AbstractVector,R1::AbstractVector,R2::AbstractVector; qQ=0, qR=0, integrator=false)
+function LQG(
+    sys::LTISystem,
+    Q1::AbstractVector,
+    Q2::AbstractVector,
+    R1::AbstractVector,
+    R2::AbstractVector;
+    qQ = 0,
+    qR = 0,
+    integrator = false,
+    kwargs...,
+)
     Q1 = diagm(0 => Q1)
     Q2 = diagm(0 => Q2)
     R1 = diagm(0 => R1)
     R2 = diagm(0 => R2)
-    integrator ? _LQGi(A,B,C,D,Q1,Q2,R1,R2, qQ, qR) : _LQG(A,B,C,D,Q1,Q2,R1,R2, qQ, qR)
+    integrator ? _LQGi(sys, Q1, Q2, R1, R2, qQ, qR; kwargs...) :
+    _LQG(sys, Q1, Q2, R1, R2, qQ, qR; kwargs...)
 end # (2) Dispatches to final
 
-# (3) For conveniece of sending a sys, dispatches to (1/2)
-LQG(sys::LTISystem, args...; kwargs...) = LQG(sys.A,sys.B,sys.C,sys.D,args...; kwargs...)
-
-
 
 # This function does the actual initialization in the standard case withput integrator
-function _LQG(A,B,C,D, Q1, Q2, R1, R2, qQ, qR)
-    n = size(A,1)
-    m = size(B,2)
-    p = size(C,1)
-    L = lqr(A, B, Q1+qQ*C'C, Q2)
-    K = kalman(A, C, R1+qR*B*B', R2)
+function _LQG(sys::LTISystem, Q1, Q2, R1, R2, qQ, qR; M = sys.C)
+    A, B, C, D = ssdata(sys)
+    n = size(A, 1)
+    m = size(B, 2)
+    p = size(C, 1)
+    L = lqr(A, B, Q1 + qQ * C'C, Q2)
+    K = kalman(A, C, R1 + qR * B * B', R2)
 
     # Controller system
-    Ac=A-B*L-K*C+K*D*L
-    Bc=K
-    Cc=L
-    Dc=zero(D')
-    sysc = ss(Ac,Bc,Cc,Dc)
+    Ac = A - B*L - K*C + K*D*L
+    Bc = K
+    Cc = L
+    Dc = zero(D')
+    sysc = ss(Ac, Bc, Cc, Dc)
 
-    return LQG(ss(A,B,C,D),Q1,Q2,R1,R2, qQ, qR, sysc, L, K, false)
+    return LQG(ss(A, B, C, D), Q1, Q2, R1, R2, qQ, qR, sysc, L, K, M, false)
 end
 
 
 # This function does the actual initialization in the integrator case
-function _LQGi(A,B,C,D, Q1, Q2, R1, R2, qQ, qR)
-    n = size(A,1)
-    m = size(B,2)
-    p = size(C,1)
+function _LQGi(sys::LTISystem, Q1, Q2, R1, R2, qQ, qR; M = sys.C)
+    A, B, C, D = ssdata(sys)
+    n = size(A, 1)
+    m = size(B, 2)
+    p = size(C, 1)
 
     # Augment with disturbance model
-    Ae = [A B; zeros(m,n+m)]
-    Be = [B;zeros(m,m)]
-    Ce = [C zeros(p,m)]
+    Ae = [A B; zeros(m, n + m)]
+    Be = [B; zeros(m, m)]
+    Ce = [C zeros(p, m)]
     De = D
 
-    L = lqr(A, B, Q1+qQ*C'C, Q2)
+    L = lqr(A, B, Q1 + qQ * C'C, Q2)
     Le = [L I]
-    K = kalman(Ae, Ce, R1+qR*Be*Be', R2)
+    K = kalman(Ae, Ce, R1 + qR * Be * Be', R2)
 
     # Controller system
-    Ac=Ae-Be*Le-K*Ce+K*De*Le
-    Bc=K
-    Cc=Le
-    Dc=zero(D')
-    sysc = ss(Ac,Bc,Cc,Dc)
+    Ac = Ae - Be*Le - K*Ce + K*De*Le
+    Bc = K
+    Cc = Le
+    Dc = zero(D')
+    sysc = ss(Ac, Bc, Cc, Dc)
 
-    LQG(ss(A,B,C,D),Q1,Q2,R1,R2, qQ, qR, sysc, Le, K, true)
+    LQG(ss(A, B, C, D), Q1, Q2, R1, R2, qQ, qR, sysc, Le, K, M, true)
 end
 
+@deprecate getindex(G::LQG, s::Symbol) getfield(G, s)
 
-function Base.getindex(G::LQG, s)
+function Base.getproperty(G::LQG, s::Symbol)
+    if s ∈ (:L, :K, :Q1, :Q2, :R1, :R2, :qQ, :qR, :integrator, :P, :M)
+        return getfield(G, s)
+    end
     s === :A && return G.P.A
     s === :B && return G.P.B
     s === :C && return G.P.C
     s === :D && return G.P.D
-    s === :L  && return G.L
-    s === :K  && return G.K
-    s === :Q1 && return G.Q1
-    s === :Q2 && return G.Q2
-    s === :R1 && return G.R1
-    s === :R2 && return G.R2
-    s === :qQ && return G.qQ
-    s === :qR && return G.qR
-    s ∈ [:sys, :P]  && return G.P
-    s ∈ [:sysc, :controller] && return G.sysc
-    s === :integrator && return G.integrator
+    s ∈ (:sys, :P) && return getfield(G, :P)
+    s ∈ (:sysc, :controller) && return getfield(G, :sysc)
 
     A = G.P.A
     B = G.P.B
     C = G.P.C
     D = G.P.D
+    M = G.M
 
     L = G.L
     K = G.K
     P = G.P
     sysc = G.sysc
 
-    n = size(A,1)
-    m = size(B,2)
-    p = size(C,1)
+    n = size(A, 1)
+    m = size(B, 2)
+    p = size(C, 1)
+    pm = size(M, 1)
 
     # Extract interesting values
     if G.integrator # Augment with disturbance model
-        A = [A B; zeros(m,n+m)]
-        B = [B;zeros(m,m)]
-        C = [C zeros(p,m)]
+        A = [A B; zeros(m, n + m)]
+        B = [B; zeros(m, m)]
+        C = [C zeros(p, m)]
+        M = [M zeros(pm, m)]
         D = D
     end
 
-    PC = P*sysc # Loop gain
+    PC = P * sysc # Loop gain
 
-    if s ∈ [:cl, :closedloop, :ry] # Closed-loop system
+    if s ∈ (:cl, :closedloop, :ry) # Closed-loop system
+        # Compensate for static gain, pp. 264 G.L.
+        dcg = P.C * ((P.B * L[:, 1:n] - P.A) \ P.B)
         Acl = [A-B*L B*L; zero(A) A-K*C]
-        Bcl = [B; zero(B)]
-        Ccl = [C zero(C)]
-        Bcl = Bcl/(P.C*inv(P.B*L[:,1:n]-P.A)*P.B) # B*lᵣ # Always normalized with nominal plant static gain
-        return syscl = ss(Acl,Bcl,Ccl,0)
-    elseif s ∈ [:Sin, :S] # Sensitivity function
-        return feedback(ss(Matrix{numeric_type(PC)}(I, m, m)),PC)
-    elseif s ∈ [:Tin, :T] # Complementary sensitivity function
+        Bcl = [B / dcg; zero(B)]
+        Ccl = [M zero(M)]
+        # rank(dcg) == size(A,1) && (Bcl = Bcl / dcg) # B*lᵣ # Always normalized with nominal plant static gain
+        syscl = ss(Acl, Bcl, Ccl, 0)
+        # return ss(A-B*L, B/dcg, M, 0)
+        return syscl
+    elseif s ∈ (:Sin, :S) # Sensitivity function
+        return feedback(ss(Matrix{numeric_type(PC)}(I, m, m)), PC)
+    elseif s ∈ (:Tin, :T) # Complementary sensitivity function
         return feedback(PC)
+        # return ss(Acl, I(size(Acl,1)), Ccl, 0)[1,2]
     elseif s === :Sout # Sensitivity function, output
-        return feedback(ss(Matrix{numeric_type(sys_c)}(I, m, m)),sysc*P)
+        return feedback(ss(Matrix{numeric_type(sysc)}(I, m, m)), sysc * P)
     elseif s === :Tout # Complementary sensitivity function, output
-        return feedback(sysc*P)
+        return feedback(sysc * P)
     elseif s === :PS # Load disturbance to output
-        return P*G[:S]
+        return P * G.S
     elseif s === :CS # Noise to control signal
-        return sysc*G[:S]
-    elseif s ∈ [:lt, :looptransfer, :loopgain]
+        return sysc * G.S
+    elseif s ∈ (:lt, :looptransfer, :loopgain)
         return PC
-    elseif s ∈ [:rd, :returndifference]
-        return  ss(Matrix{numeric_type(PC)}(I, p, p)) + PC
-    elseif s ∈ [:sr, :stabilityrobustness]
-        return  ss(Matrix{numeric_type(PC)}(I, p, p)) + inv(PC)
+    elseif s ∈ (:rd, :returndifference)
+        return ss(Matrix{numeric_type(PC)}(I, p, p)) + PC
+    elseif s ∈ (:sr, :stabilityrobustness)
+        return ss(Matrix{numeric_type(PC)}(I, p, p)) + inv(PC)
     end
     error("The symbol $s does not have a function associated with it.")
 end
 
-Base.:(==)(G1::LQG, G2::LQG) = G1.K == G2.K && G1.L == G2.L && G1.P == G2.P && G1.sysc == G2.sysc
+Base.:(==)(G1::LQG, G2::LQG) =
+    G1.K == G2.K && G1.L == G2.L && G1.P == G2.P && G1.sysc == G2.sysc
 
 
 plot(G::LQG) = gangoffourplot(G)
+
 function gangoffour(G::LQG)
-    return G[:S], G[:PS], G[:CS], G[:T]
+    G.S, G.PS, G.CS, G.T
 end
 
 function gangoffourplot(G::LQG; kwargs...)

test/test_lqg.jl

@@ -30,16 +30,16 @@ Gi = LQG(sys, Q1, Q2, R1, R2, qQ=qQ, qR=qR, integrator=true)
 gangoffourplot(Gi) # Test that it at least does not error
 @test approxsetequal(eigvals(Gi.sysc.A), [0.0, 0.0, -47.4832, -44.3442, -3.40255, -1.15355 ], rtol = 1e-3)
 
-@test approxsetequal(eigvals(G[:cl].A), [-1.0, -14.1774, -2.21811, -14.3206, -1.60615, -22.526, -1.0, -14.1774], rtol=1e-3)
-@test approxsetequal(eigvals(G[:T].A), [-22.526, -2.21811, -1.60615, -14.3206, -14.1774, -1.0, -1.0, -14.1774], rtol=1e-3)
-@test approxsetequal(eigvals(G[:S].A), [-22.526, -2.21811, -1.60615, -14.3206, -14.1774, -1.0, -1.0, -14.1774], rtol=1e-3)
-@test approxsetequal(eigvals(G[:CS].A), [-31.6209+0.0im, -1.40629+0.0im, -15.9993+0.911174im, -15.9993-0.911174im, -22.526+0.0im, -2.21811+0.0im, -1.60615+0.0im, -14.3206+0.0im, -14.1774+0.0im, -1.0+0.0im, -1.0+0.0im, -14.1774+0.0im], rtol=1e-3)
-@test approxsetequal(eigvals(G[:PS].A), [-1.0, -1.0, -3.0, -1.0, -22.526, -2.21811, -1.60615, -14.3206, -14.1774, -1.0, -1.0, -14.1774], rtol=1e-3)
+@test approxsetequal(eigvals(G.cl.A), [-1.0, -14.1774, -2.21811, -14.3206, -1.60615, -22.526, -1.0, -14.1774], rtol=1e-3)
+@test approxsetequal(eigvals(G.T.A), [-22.526, -2.21811, -1.60615, -14.3206, -14.1774, -1.0, -1.0, -14.1774], rtol=1e-3)
+@test approxsetequal(eigvals(G.S.A), [-22.526, -2.21811, -1.60615, -14.3206, -14.1774, -1.0, -1.0, -14.1774], rtol=1e-3)
+@test approxsetequal(eigvals(G.CS.A), [-31.6209+0.0im, -1.40629+0.0im, -15.9993+0.911174im, -15.9993-0.911174im, -22.526+0.0im, -2.21811+0.0im, -1.60615+0.0im, -14.3206+0.0im, -14.1774+0.0im, -1.0+0.0im, -1.0+0.0im, -14.1774+0.0im], rtol=1e-3)
+@test approxsetequal(eigvals(G.PS.A), [-1.0, -1.0, -3.0, -1.0, -22.526, -2.21811, -1.60615, -14.3206, -14.1774, -1.0, -1.0, -14.1774], rtol=1e-3)
 
 
-@test approxsetequal(eigvals(Gi[:cl].A), [-1.0, -44.7425, -44.8455, -2.23294, -4.28574, -2.06662, -0.109432, -1.31779, -0.78293, -1.0, 0.0, 0.0], rtol=1e-3)
-@test approxsetequal(eigvals(Gi[:T].A), [-44.7425, -44.8455, -4.28574, -0.109432, -2.23294, -2.06662, -1.31779, -0.78293, -1.0, -1.0], rtol=1e-3)
-@test approxsetequal(eigvals(Gi[:S].A), [-44.7425, -44.8455, -4.28574, -0.109432, -2.23294, -2.06662, -1.31779, -0.78293, -1.0, -1.0], rtol=1e-3)
+@test approxsetequal(eigvals(Gi.cl.A), [-1.0, -44.7425, -44.8455, -2.23294, -4.28574, -2.06662, -0.109432, -1.31779, -0.78293, -1.0, 0.0, 0.0], rtol=1e-3)
+@test approxsetequal(eigvals(Gi.T.A), [-44.7425, -44.8455, -4.28574, -0.109432, -2.23294, -2.06662, -1.31779, -0.78293, -1.0, -1.0], rtol=1e-3)
+@test approxsetequal(eigvals(Gi.S.A), [-44.7425, -44.8455, -4.28574, -0.109432, -2.23294, -2.06662, -1.31779, -0.78293, -1.0, -1.0], rtol=1e-3)