Merge pull request #259 from olof3/delay_work

Padé approximations, General feedback interconnections, test systems

Author: mfalt

README.md

@@ -73,7 +73,7 @@ A documentation website is available at [http://juliacontrol.github.io/ControlSy
 
 Some of the available commands are:
 ##### Constructing systems
-ss, tf, zpk, ss2tf
+ss, tf, zpk
 ##### Analysis
 pole, tzero, norm, hinfnorm, linfnorm, ctrb, obsv, gangoffour, margin, markovparam, damp, dampreport, zpkdata, dcgain, covar, gram, sigma, sisomargin
 ##### Synthesis

src/ControlSystems.jl

@@ -9,8 +9,8 @@ export  LTISystem,
         ss,
         tf,
         zpk,
-        ss2tf,
         LQG,
+        isproper,
         # Linear Algebra
         balance,
         care,
@@ -56,6 +56,8 @@ export  LTISystem,
         parallel,
         feedback,
         feedback2dof,
+        starprod,
+        lft,
         # Discrete
         c2d,
         # Time Response
@@ -72,6 +74,10 @@ export  LTISystem,
         sigma,
         # delay systems
         delay,
+        pade,
+        # demo systems
+        ssrand,
+        DemoSystems, # A module containing some example systems
         # utilities
         num,    #Deprecated
         den,    #Deprecated
@@ -89,6 +95,7 @@ using OrdinaryDiffEq, DelayDiffEq
 export Plots
 import Base: +, -, *, /, (==), (!=), isapprox, convert, promote_op
 import Base: getproperty
+import Base: exp # for exp(-s)
 import LinearAlgebra: BlasFloat
 export lyap # Make sure LinearAlgebra.lyap is available
 import Printf, Colors
@@ -138,6 +145,8 @@ include("synthesis.jl")
 include("simulators.jl")
 include("pid_design.jl")
 
+include("demo_systems.jl")
+
 include("delay_systems.jl")
 
 include("plotting.jl")
@@ -146,6 +155,12 @@ include("plotting.jl")
 @deprecate den denvec
 @deprecate norminf hinfnorm
 
+function covar(D::Union{AbstractMatrix,UniformScaling}, R)
+    @warn "This call is deprecated due to ambiguity, use covar(ss(D), R) or covar(ss(D, Ts), R) instead"
+    D*R*D'
+end
+
+
 # The path has to be evaluated upon initial import
 const __CONTROLSYSTEMS_SOURCE_DIR__ = dirname(Base.source_path())
 

src/connections.jl

@@ -203,3 +203,219 @@ function blockdiag(mats::AbstractMatrix{T}...) where T
     end
     return res
 end
+
+
+
+"""
+`feedback(L)` Returns L/(1+L)
+`feedback(P1,P2)` Returns P1/(1+P1*P2)
+"""
+feedback(L::TransferFunction) = L/(1+L)
+feedback(P1::TransferFunction, P2::TransferFunction) = P1/(1+P1*P2)
+
+#Efficient implementations
+function feedback(L::TransferFunction{T}) where T<:SisoRational
+    if size(L) != (1,1)
+        error("MIMO TransferFunction feedback isn't implemented, use L/(1+L)")
+    end
+    P = numpoly(L)
+    Q = denpoly(L)
+    tf(P, P+Q, L.Ts)
+end
+
+function feedback(L::TransferFunction{T}) where {T<:SisoZpk}
+    if size(L) != (1,1)
+        error("MIMO TransferFunction feedback isn't implemented, use L/(1+L)")
+    end
+    #Extract polynomials and create P/(P+Q)
+    k = L.matrix[1].k
+    denpol = numpoly(L)[1]+denpoly(L)[1]
+    kden = denpol[end] # Get coeff of s^n
+    # Create siso system
+    sisozpk = T(L.matrix[1].z, roots(denpol), k/kden)
+    return TransferFunction{T}(fill(sisozpk,1,1), L.Ts)
+end
+
+"""
+`feedback(sys)`
+
+`feedback(sys1,sys2)`
+
+Forms the negative feedback interconnection
+```julia
+>-+ sys1 +-->
+  |      |
+ (-)sys2 +
+```
+If no second system is given, negative identity feedback is assumed
+"""
+function feedback(sys::Union{StateSpace, DelayLtiSystem})
+    ninputs(sys) != noutputs(sys) && error("Use feedback(sys1, sys2) if number of inputs != outputs")
+    feedback(sys,ss(Matrix{numeric_type(sys)}(I,size(sys)...)))
+end
+
+function feedback(sys1::StateSpace,sys2::StateSpace)
+    if sys1.Ts != sys2.Ts # FIXME: replace with common_sample_time
+        error("Sampling time mismatch")
+    end
+
+    !(iszero(sys1.D) || iszero(sys2.D)) && error("There cannot be a direct term (D) in both sys1 and sys2")
+    A = [sys1.A+sys1.B*(-sys2.D)*sys1.C sys1.B*(-sys2.C);
+         sys2.B*sys1.C  sys2.A+sys2.B*sys1.D*(-sys2.C)]
+    B = [sys1.B; sys2.B*sys1.D]
+    C = [sys1.C  sys1.D*(-sys2.C)]
+
+    ss(A, B, C, sys1.D, sys1.Ts)
+end
+
+
+"""
+    feedback(s1::AbstractStateSpace, s2::AbstractStateSpace;
+             U1=:, Y1=:, U2=:, Y2=:, W1=:, Z1=:, W2=Int[], Z2=Int[],
+             Wperm=:, Zperm=:, pos_feedback::Bool=false)
+
+
+`U1`, `Y1`, `U2`, `Y2` contain the indices of the signals that should be connected.
+`W1`, `Z1`, `W2`, `Z2` contain the signal indices of `s1` and `s2` that should be kept.
+
+Specify  `Wperm` and `Zperm` to reorder [w1; w2] and [z1; z2] in the resulting statespace model.
+
+Negative feedback is the default. Specify `pos_feedback=true` for positive feedback.
+
+See Zhou etc. for similar (somewhat less symmetric) formulas.
+"""
+@views function feedback(sys1::AbstractStateSpace, sys2::AbstractStateSpace;
+    U1=:, Y1=:, U2=:, Y2=:, W1=:, Z1=:, W2=Int[], Z2=Int[],
+    Wperm=:, Zperm=:, pos_feedback::Bool=false)
+
+    if sys1.Ts != sys2.Ts # FIXME: replace with common_sample_time
+        error("Sampling time mismatch")
+    end
+
+    if !(isa(Y1, Colon) || allunique(Y1)); @warn "Connecting single output to multiple inputs Y1=$Y1"; end
+    if !(isa(Y2, Colon) || allunique(Y2)); @warn "Connecting single output to multiple inputs Y2=$Y2"; end
+    if !(isa(U1, Colon) || allunique(U1)); @warn "Connecting multiple outputs to a single input U1=$U1"; end
+    if !(isa(U2, Colon) || allunique(U2)); @warn "Connecting a single output to multiple inputs U2=$U2"; end
+
+    if (U1 isa Colon ? size(sys1, 2) : length(U1)) != (Y2 isa Colon ? size(sys2, 1) : length(Y2))
+        error("Lengths of U1 ($U1) and Y2 ($Y2) must be equal")
+    end
+    if (U2 isa Colon ? size(sys2, 2) : length(U2)) != (Y1 isa Colon ? size(sys1, 1) : length(Y1))
+        error("Lengths of U1 ($U2) and Y2 ($Y1) must be equal")
+    end
+
+    α = pos_feedback ? 1 : -1 # The sign of feedback
+
+    s1_B1 = sys1.B[:,W1]
+    s1_B2 = sys1.B[:,U1]
+    s1_C1 = sys1.C[Z1,:]
+    s1_C2 = sys1.C[Y1,:]
+    s1_D11 = sys1.D[Z1,W1]
+    s1_D12 = sys1.D[Z1,U1]
+    s1_D21 = sys1.D[Y1,W1]
+    s1_D22 = sys1.D[Y1,U1]
+
+    s2_B1 = sys2.B[:,W2]
+    s2_B2 = sys2.B[:,U2]
+    s2_C1 = sys2.C[Z2,:]
+    s2_C2 = sys2.C[Y2,:]
+    s2_D11 = sys2.D[Z2,W2]
+    s2_D12 = sys2.D[Z2,U2]
+    s2_D21 = sys2.D[Y2,W2]
+    s2_D22 = sys2.D[Y2,U2]
+
+    if iszero(s1_D22) || iszero(s2_D22)
+        A = [sys1.A + α*s1_B2*s2_D22*s1_C2        α*s1_B2*s2_C2;
+                 s2_B2*s1_C2            sys2.A + α*s2_B2*s1_D22*s2_C2]
+
+        B = [s1_B1 + α*s1_B2*s2_D22*s1_D21        α*s1_B2*s2_D21;
+                      s2_B2*s1_D21            s2_B1 + α*s2_B2*s1_D22*s2_D21]
+        C = [s1_C1 + α*s1_D12*s2_D22*s1_C2        α*s1_D12*s2_C2;
+                      s2_D12*s1_C2           s2_C1 + α*s2_D12*s1_D22*s2_C2]
+        D = [s1_D11 + α*s1_D12*s2_D22*s1_D21        α*s1_D12*s2_D21;
+                      s2_D12*s1_D21           s2_D11 + α*s2_D12*s1_D22*s2_D21]
+    else
+        # inv seems to be better than lu
+        R1 = try
+            inv(α*I - s2_D22*s1_D22) # slightly faster than α*inv(I - α*s2_D22*s1_D22)
+        catch
+            error("Ill-posed feedback interconnection,  I - α*s2_D22*s1_D22 or I - α*s2_D22*s1_D22 not invertible")
+        end
+
+        R2 = try
+            inv(I - α*s1_D22*s2_D22)
+        catch
+            error("Ill-posed feedback interconnection,  I - α*s2_D22*s1_D22 or I - α*s2_D22*s1_D22 not invertible")
+        end
+
+        A = [sys1.A + s1_B2*R1*s2_D22*s1_C2        s1_B2*R1*s2_C2;
+                 s2_B2*R2*s1_C2            sys2.A + α*s2_B2*R2*s1_D22*s2_C2]
+
+        B = [s1_B1 + s1_B2*R1*s2_D22*s1_D21        s1_B2*R1*s2_D21;
+                     s2_B2*R2*s1_D21            s2_B1 + α*s2_B2*R2*s1_D22*s2_D21]
+        C = [s1_C1 + s1_D12*R1*s2_D22*s1_C2        s1_D12*R1*s2_C2;
+                     s2_D12*R2*s1_C2           s2_C1 + α*s2_D12*R2*s1_D22*s2_C2]
+        D = [s1_D11 + s1_D12*R1*s2_D22*s1_D21        s1_D12*R1*s2_D21;
+                     s2_D12*R2*s1_D21           s2_D11 + α*s2_D12*R2*s1_D22*s2_D21]
+    end
+
+    return StateSpace(A, B[:, Wperm], C[Zperm,:], D[Zperm, Wperm], sys1.Ts)
+end
+
+
+"""
+`feedback2dof(P,R,S,T)` Return `BT/(AR+ST)` where B and A are the numerator and denomenator polynomials of `P` respectively
+`feedback2dof(B,A,R,S,T)` Return `BT/(AR+ST)`
+"""
+function feedback2dof(P::TransferFunction,R,S,T)
+    !issiso(P) && error("Feedback not implemented for MIMO systems")
+    tf(conv(poly2vec(numpoly(P)[1]),T),zpconv(poly2vec(denpoly(P)[1]),R,poly2vec(numpoly(P)[1]),S))
+end
+
+feedback2dof(B,A,R,S,T) = tf(conv(B,T),zpconv(A,R,B,S))
+
+
+"""
+    lft(G, Δ, type=:l)
+
+Lower and upper linear fractional transformation between systems `G` and `Δ`.
+
+Specify `:l` lor lower LFT, and `:u` for upper LFT.
+
+`G` must have more inputs and outputs than `Δ` has outputs and inputs.
+
+For details, see Chapter 9.1 in
+**Zhou, K. and JC Doyle**. Essentials of robust control, Prentice hall (NJ), 1998
+"""
+function lft(G, Δ, type=:l)
+
+    if !(G.nu > Δ.ny && G.ny > Δ.nu)
+        error("Must have G.nu > Δ.ny and G.ny > Δ.nu for lower/upper lft")
+    end
+
+    if type == :l
+        feedback(G, Δ, U1=G.nu-Δ.ny+1:G.nu, Y1=G.ny-Δ.nu+1:G.ny, W1=1:G.ny-Δ.nu, Z1=1:G.nu-Δ.ny, pos_feedback=true)
+    elseif type == :u
+        feedback(G, Δ, U1=1:Δ.ny, Y1=1:Δ.nu, W1=Δ.nu+1:G.ny, Z1=Δ.nu+1:G.ny, pos_feedback=true)
+    else
+        error("Invalid type of lft ($type), specify type=:l (:u) for lower (upper) lft")
+    end
+end
+
+
+
+"""
+    starprod(sys1, sys2, dimu, dimy)
+
+Compute the Redheffer star product.
+
+`length(U1) = length(Y2) = dimu` and `length(Y1) = length(U2) = dimy`
+
+For details, see Chapter 9.3 in
+**Zhou, K. and JC Doyle**. Essentials of robust control, Prentice hall (NJ), 1998
+"""
+starprod(G1, G2, dimy::Int, dimu::Int) = feedback(G1, G2,
+         U1=G1.nu-dimu+1:G1.nu, Y1=G1.ny-dimy+1:G1.ny, W1=1:G1.nu-dimu, Z1=1:G1.ny-dimy,
+         U2=1:dimy, Y2=1:dimu, W2=dimy+1:G2.nu, Z2=dimu+1:G2.ny,
+         pos_feedback=true)
+starprod(sys1, sys2) = lft(sys1, sys2, :l)

src/delay_systems.jl

@@ -12,7 +12,7 @@ function freqresp(sys::DelayLtiSystem, ω::AbstractVector{T}) where {T <: Real}
         P21_fr = P_fr[ω_idx, ny+1:end, 1:nu]
         P22_fr = P_fr[ω_idx, ny+1:end, nu+1:end]
 
-        delay_matrix_inv_fr = Diagonal(exp.(im*sys.Tau*ω[ω_idx])) # Frequency response of the diagonal matrix with delays
+        delay_matrix_inv_fr = Diagonal(exp.(im*ω[ω_idx]*sys.Tau)) # Frequency response of the diagonal matrix with delays
         # Inverse of the delay matrix, so there should not be any minus signs in the exponents
 
         G_fr[ω_idx,:,:] .= P11_fr + P12_fr/(delay_matrix_inv_fr - P22_fr)*P21_fr # The matrix is invertible (?!)
@@ -21,6 +21,20 @@ function freqresp(sys::DelayLtiSystem, ω::AbstractVector{T}) where {T <: Real}
     return G_fr
 end
 
+function evalfr(sys::DelayLtiSystem, s)
+    (ny, nu) = size(sys)
+
+    P_fr = evalfr(sys.P.P, s)
+
+    P11_fr = P_fr[1:ny, 1:nu]
+    P12_fr = P_fr[1:ny, nu+1:end]
+    P21_fr = P_fr[ny+1:end, 1:nu]
+    P22_fr = P_fr[ny+1:end, nu+1:end]
+
+    delay_matrix_inv_fr = Diagonal(exp.(s*sys.Tau))
+
+    return P11_fr + P12_fr/(delay_matrix_inv_fr - P22_fr)*P21_fr
+end
 
 """
     `y, t, x = lsim(sys::DelayLtiSystem, u, t::AbstractArray{<:Real}; x0=fill(0.0, nstates(sys)), alg=MethodOfSteps(Tsit5()), kwargs...)`
@@ -201,3 +215,62 @@ function impulse(sys::DelayLtiSystem{T}, t::AbstractVector; alg=MethodOfSteps(BS
     end
     return y, tout, x
 end
+
+
+# Used for pade approximation
+"""
+`p2 = _linscale(p::Poly, a)`
+
+Given a polynomial `p` and a number `a, returns the polynomials `p2` such that
+`p2(s) == p(a*s)`.
+"""
+function _linscale(p::Poly, a)
+    # This function should perhaps be implemented in Polynomials.jl
+    coeffs_scaled = similar(p.a, promote_type(eltype(p), typeof(a)))
+    a_pow = 1
+    coeffs_scaled[1] = p.a[1]
+    for k=2:length(p.a)
+        a_pow *= a
+        coeffs_scaled[k] = p.a[k]*a_pow
+    end
+    return Poly(coeffs_scaled)
+end
+
+# Coefficeints for Padé approximations
+# PADE_Q_COEFFS = [Poly([binomial(N,i)*prod(N+1:2*N-i) for i=0:N]) for N=1:10]
+const PADE_Q_COEFFS =  [[2, 1],
+ [12, 6, 1],
+ [120, 60, 12, 1],
+ [1680, 840, 180, 20, 1],
+ [30240, 15120, 3360, 420, 30, 1],
+ [665280, 332640, 75600, 10080, 840, 42, 1],
+ [17297280, 8648640, 1995840, 277200, 25200, 1512, 56, 1],
+ [518918400, 259459200, 60540480, 8648640, 831600, 55440, 2520, 72, 1],
+ [17643225600, 8821612800, 2075673600, 302702400, 30270240, 2162160, 110880, 3960, 90, 1],
+ [670442572800, 335221286400, 79394515200, 11762150400, 1210809600, 90810720, 5045040, 205920, 5940, 110, 1]]
+
+"""
+    pade(τ::Real, N::Int)
+
+Compute the `N`th order Padé approximation of a time-delay of length `τ`.
+"""
+function pade(τ::Real, N::Int)
+    if !(1 <= N <= 10); error("Order of Padé approximation must be between 1 and 10. Got $N."); end
+
+    Q = Poly(PADE_Q_COEFFS[N])
+
+    return tf(_linscale(Q, -τ), _linscale(Q, τ)) # return Q(-τs)/Q(τs)
+end
+
+
+"""
+    pade(G::DelayLtiSystem, N)
+
+Approximate all time-delays in `G` by Padé approximations of degree `N`.
+"""
+function pade(G::DelayLtiSystem, N)
+    ny, nu = size(G)
+    nTau = length(G.Tau)
+    X = append([ss(pade(τ,N)) for τ in G.Tau]...) # Perhaps append should be renamed blockdiag
+    return lft(G.P.P, X)
+end