Work on Padé approximations and general feedback interconnections

Author: olof3

src/ControlSystems.jl

@@ -11,6 +11,7 @@ export  LTISystem,
         zpk,
         ss2tf,
         LQG,
+        isproper,
         # Linear Algebra
         balance,
         care,
@@ -56,6 +57,8 @@ export  LTISystem,
         parallel,
         feedback,
         feedback2dof,
+        starprod,
+        lft,
         # Discrete
         c2d,
         # Time Response
@@ -72,6 +75,7 @@ export  LTISystem,
         sigma,
         # delay systems
         delay,
+        pade,
         # utilities
         num,    #Deprecated
         den,    #Deprecated
@@ -89,6 +93,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

src/connections.jl

@@ -203,3 +203,202 @@ 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)
+    sum(abs.(sys1.D)) != 0 && sum(abs.(sys2.D)) != 0 && error("There can not 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)
+end
+
+
+"""
+    feedback(s1::AbstractStateSpace, s2::AbstractStateSpace;
+        U1=1:size(s1,2), Y1=1:size(s1,1), U2=1:size(s2,2), Y2=1:size(s2,1),
+        W1=1:size(s1,2), Z1=1:size(s1,1), W2=Int[], Z2=Int[],
+        w_reorder=nothing, z_reorder=nothing,
+        neg_feeback::Bool=true)
+
+
+`U1`, `Y1`, `U2`, `Y2` contains the indices of the signals that should be connected.
+`W1`, `Z1`, `W2`, `Z2` contains the signal indices of `s1` and `s2` that should be kept.
+
+Specify  `Wreorder` and `Zreorder` to reorder [W1; W2] and [Z1; Z2] after matrix multiplications.
+
+Negative feedback is the default. Specify `neg_feedback=false` for positive feedback.
+
+See Zhou etc. for similar (somewhat less symmetric) formulas.
+"""
+function feedback(s1::AbstractStateSpace, s2::AbstractStateSpace;
+    U1=1:size(s1,2), Y1=1:size(s1,1), U2=1:size(s2,2), Y2=1:size(s2,1),
+    W1=1:size(s1,2), Z1=1:size(s1,1), W2=Int[], Z2=Int[],
+    w_reorder=nothing, z_reorder=nothing,
+    neg_feedback::Bool=true)
+
+    if !(allunique(Y1) && allunique(Y2))
+        warning("Connecting a single output to multiple inputs.")
+    end
+    if !(allunique(U1) && allunique(U2))
+        warning("Connecting multiple outputs to a single input.")
+    end
+
+    α = neg_feedback ? -1 : 1 # The sign of feedback
+
+    s1_B1 = s1.B[:,W1]
+    s1_B2 = s1.B[:,U1]
+    s1_C1 = s1.C[Z1,:]
+    s1_C2 = s1.C[Y1,:]
+    s1_D11 = s1.D[Z1,W1]
+    s1_D12 = s1.D[Z1,U1]
+    s1_D21 = s1.D[Y1,W1]
+    s1_D22 = s1.D[Y1,U1]
+
+    s2_B1 = s2.B[:,W2]
+    s2_B2 = s2.B[:,U2]
+    s2_C1 = s2.C[Z2,:]
+    s2_C2 = s2.C[Y2,:]
+    s2_D11 = s2.D[Z2,W2]
+    s2_D12 = s2.D[Z2,U2]
+    s2_D21 = s2.D[Y2,W2]
+    s2_D22 = s2.D[Y2,U2]
+
+
+    if iszero(s1_D22) || iszero(s2_D22)
+        A = [s1.A + α*s1_B2*s2_D22*s1_C2        α*s1_B2*s2_C2;
+                 s2_B2*s1_C2            s2.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
+        R1 = inv(I - α*s2_D22*s1_D22) # inv seems to be better than lu
+        R2 = inv(I - α*s1_D22*s2_D22)
+
+        A = [s1.A + α*s1_B2*R1*s2_D22*s1_C2        α*s1_B2*R1*s2_C2;
+                 s2_B2*R2*s1_C2            s2.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 while reorganizing into the desired order
+    return StateSpace(A, isnothing(w_reorder) ? B : B[:, w_reorder],
+                         isnothing(z_reorder) ? C : C[z_reorder,:],
+                         isnothing(w_reorder) && isnothing(z_reorder) ? D : D[z_reorder, w_reorder])
+    #return StateSpace(A, B_tmp, C_tmp, D_tmp)
+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)
+
+Upper linear fractional transformation between systems `G` and `Δ`.
+`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) # QUESTION: Or lft_u, and lft_l instead?
+
+    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, neg_feedback=false)
+    elseif type == :u
+        feedback(G, Δ, U1=1:Δ.ny, Y1=1:Δ.nu, W1=Δ.nu+1:G.ny, Z1=Δ.nu+1:G.ny, neg_feedback=false)
+    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
+"""
+function 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,
+                     neg_feedback=false)
+end
+function starprod(sys1, sys2)
+    nu = size(sys2, 2)
+    ny = size(sys2, 1)
+    starp(sys1, sys2, nu, ny)
+end

src/delay_systems.jl

@@ -201,3 +201,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
+# 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

src/synthesis.jl

@@ -143,72 +143,3 @@ function acker(A,B,P)
     end
     return [zeros(1,n-1) 1]*(S\q)
 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)
-    sum(abs.(sys1.D)) != 0 && sum(abs.(sys2.D)) != 0 && error("There can not 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)
-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))