|
| 1 | +const StaticStateSpace{TE, ny, nu, nx} = HeteroStateSpace{TE, |
| 2 | + <:SMatrix{nx,nx}, |
| 3 | + <:SMatrix{nx,nu}, |
| 4 | + <:SMatrix{ny,nx}, |
| 5 | + <:SMatrix{ny,nu} |
| 6 | +} |
| 7 | + |
| 8 | + |
| 9 | +to_static(a::Number) = a |
| 10 | +to_static(a::AbstractArray) = SMatrix{size(a,1), size(a,2), eltype(a), prod(size(a))}(a) |
| 11 | +to_static(a::SArray) = a |
| 12 | +to_sized(a::Number) = a |
| 13 | +to_sized(a::AbstractArray) = SizedArray{Tuple{size(a)...}}(a) |
| 14 | + |
| 15 | +function HeteroStateSpace(A,B,C,D,Ts=0,f::F=to_static) where F |
| 16 | + HeteroStateSpace(f(A),f(B),f(C),f(D),Ts) |
| 17 | +end |
| 18 | + |
| 19 | +HeteroStateSpace(s,f) = HeteroStateSpace(s.A,s.B,s.C,s.D,s.timeevol,f) |
| 20 | + |
| 21 | +ControlSystems._string_mat_with_headers(a::SizedArray) = ControlSystems._string_mat_with_headers(Matrix(a)) |
| 22 | + |
| 23 | + |
| 24 | + |
| 25 | +""" |
| 26 | + StaticStateSpace(sys::AbstractStateSpace) |
| 27 | +
|
| 28 | +Return a [`HeteroStateSpace`](@ref) where the system matrices are of type SMatrix. |
| 29 | +
|
| 30 | +*NOTE: This function is fundamentally type unstable.* For maximum performance, create the static system manually, or make use of the function-barrier technique. |
| 31 | +""" |
| 32 | +StaticStateSpace(sys::AbstractStateSpace) = HeteroStateSpace(sys, to_static) |
| 33 | + |
| 34 | +""" |
| 35 | + to_sized(sys::AbstractStateSpace) |
| 36 | +
|
| 37 | +Return a [`HeteroStateSpace`](@ref) where the system matrices are of type SizedMatrix. |
| 38 | +
|
| 39 | +*NOTE: This function is fundamentally type unstable.* For maximum performance, create the sized system manually, or make use of the function-barrier technique. |
| 40 | +""" |
| 41 | +to_sized(sys::AbstractStateSpace) = HeteroStateSpace(sys, to_sized) |
| 42 | + |
| 43 | +StaticStateSpace(G::TransferFunction) = StaticStateSpace(ss(G)) |
| 44 | + |
| 45 | +# function to_static(sys::DelayLtiSystem) |
| 46 | +# innerP = to_static(sys.P.P) |
| 47 | +# partP = PartitionedStateSpace(innerP, sys.P.nu1, sys.P.ny1) |
| 48 | +# DelayLtiSystem(partP, sys.Tau) |
| 49 | +# end |
| 50 | + |
| 51 | + |
| 52 | +## Feedback |
| 53 | +# Special method to make sure we handle static systems in a type stable way |
| 54 | + |
| 55 | + |
| 56 | +function HeteroStateSpace(D::SArray, Ts=nothing) |
| 57 | + HeteroStateSpace(D, Ts === nothing ? Continuous() : Discrete(Ts)) |
| 58 | +end |
| 59 | + |
| 60 | +function HeteroStateSpace(D::SArray, timeevol::TimeEvolution) |
| 61 | + HeteroStateSpace( |
| 62 | + SMatrix{0,0,eltype(D),0}(), |
| 63 | + SMatrix{0,size(D,2),eltype(D),0}(), |
| 64 | + SMatrix{size(D,1),0,eltype(D),0}(), |
| 65 | + D, |
| 66 | + timeevol |
| 67 | + ) |
| 68 | +end |
| 69 | + |
| 70 | +function StaticStateSpace(D::Array, args...) |
| 71 | + HeteroStateSpace(to_static(D), args...) |
| 72 | +end |
| 73 | + |
| 74 | +function Base.promote_rule(::Type{N}, ::Type{<:HeteroStateSpace{TE}}) where {N <: Number, TE} |
| 75 | + HeteroStateSpace{TE} |
| 76 | +end |
| 77 | + |
| 78 | +function Base.convert(::Type{<:HeteroStateSpace{TE}}, n::Number) where TE |
| 79 | + if TE <: Continuous |
| 80 | + HeteroStateSpace(SMatrix{1,1,typeof(n),1}(n), Continuous()) |
| 81 | + else |
| 82 | + HeteroStateSpace(SMatrix{1,1,typeof(n),1}(n), Discrete(UNDEF_SAMPLEPETIME)) |
| 83 | + end |
| 84 | +end |
| 85 | + |
| 86 | +function Base.promote_rule(::Type{StateSpace{TE1,T}}, ::Type{HeteroStateSpace{TE2,AT,BT,CT,DT}}) where {TE1,TE2,T,AT<:SMatrix,BT<:SMatrix,CT<:SMatrix,DT<:SMatrix} |
| 87 | + StateSpace{promote_type(TE1, TE2), promote_type(T, eltype(AT), eltype(BT), eltype(CT), eltype(DT))} |
| 88 | +end |
| 89 | + |
| 90 | + |
| 91 | +function Base.promote_rule(::Type{TransferFunction{TE1,T}}, ::Type{HeteroStateSpace{TE2,AT,BT,CT,DT}}) where {TE1,TE2,T,AT<:SMatrix,BT<:SMatrix,CT<:SMatrix,DT<:SMatrix} |
| 92 | + StateSpace{promote_type(TE1, TE2), promote_type(numeric_type(T), eltype(AT), eltype(BT), eltype(CT), eltype(DT))} |
| 93 | +end |
| 94 | + |
| 95 | + |
| 96 | +function feedback(sys1::StaticStateSpace, sys2::StaticStateSpace; |
| 97 | + U1=:, Y1=:, U2=:, Y2=:, W1=:, Z1=:, W2=SVector{0,Int}(), Z2=SVector{0,Int}(), |
| 98 | + Wperm=:, Zperm=:, pos_feedback::Bool=false) |
| 99 | + |
| 100 | + timeevol = common_timeevol(sys1,sys2) |
| 101 | + |
| 102 | + if !(isa(Y1, Colon) || allunique(Y1)); @warn "Connecting single output to multiple inputs Y1=$Y1"; end |
| 103 | + if !(isa(Y2, Colon) || allunique(Y2)); @warn "Connecting single output to multiple inputs Y2=$Y2"; end |
| 104 | + if !(isa(U1, Colon) || allunique(U1)); @warn "Connecting multiple outputs to a single input U1=$U1"; end |
| 105 | + if !(isa(U2, Colon) || allunique(U2)); @warn "Connecting a single output to multiple inputs U2=$U2"; end |
| 106 | + |
| 107 | + if (U1 isa Colon ? size(sys1, 2) : length(U1)) != (Y2 isa Colon ? size(sys2, 1) : length(Y2)) |
| 108 | + error("Lengths of U1 ($U1) and Y2 ($Y2) must be equal") |
| 109 | + end |
| 110 | + if (U2 isa Colon ? size(sys2, 2) : length(U2)) != (Y1 isa Colon ? size(sys1, 1) : length(Y1)) |
| 111 | + error("Lengths of U1 ($U2) and Y2 ($Y1) must be equal") |
| 112 | + end |
| 113 | + |
| 114 | + α = pos_feedback ? 1 : -1 # The sign of feedback |
| 115 | + |
| 116 | + s1_B1 = sys1.B[:,W1] |
| 117 | + s1_B2 = sys1.B[:,U1] |
| 118 | + s1_C1 = sys1.C[Z1,:] |
| 119 | + s1_C2 = sys1.C[Y1,:] |
| 120 | + s1_D11 = sys1.D[Z1,W1] |
| 121 | + s1_D12 = sys1.D[Z1,U1] |
| 122 | + s1_D21 = sys1.D[Y1,W1] |
| 123 | + s1_D22 = sys1.D[Y1,U1] |
| 124 | + |
| 125 | + s2_B1 = sys2.B[:,W2] |
| 126 | + s2_B2 = sys2.B[:,U2] |
| 127 | + s2_C1 = sys2.C[Z2,:] |
| 128 | + s2_C2 = sys2.C[Y2,:] |
| 129 | + s2_D11 = sys2.D[Z2,W2] |
| 130 | + s2_D12 = sys2.D[Z2,U2] |
| 131 | + s2_D21 = sys2.D[Y2,W2] |
| 132 | + s2_D22 = sys2.D[Y2,U2] |
| 133 | + |
| 134 | + if iszero(s1_D22) || iszero(s2_D22) |
| 135 | + A = [[sys1.A + α*s1_B2*s2_D22*s1_C2 α*s1_B2*s2_C2]; |
| 136 | + [s2_B2*s1_C2 sys2.A + α*s2_B2*s1_D22*s2_C2]] |
| 137 | + |
| 138 | + B = [[s1_B1 + α*s1_B2*s2_D22*s1_D21 α*s1_B2*s2_D21]; |
| 139 | + [s2_B2*s1_D21 s2_B1 + α*s2_B2*s1_D22*s2_D21]] |
| 140 | + C = [[s1_C1 + α*s1_D12*s2_D22*s1_C2 α*s1_D12*s2_C2]; |
| 141 | + [s2_D12*s1_C2 s2_C1 + α*s2_D12*s1_D22*s2_C2]] |
| 142 | + D = [[s1_D11 + α*s1_D12*s2_D22*s1_D21 α*s1_D12*s2_D21]; |
| 143 | + [s2_D12*s1_D21 s2_D11 + α*s2_D12*s1_D22*s2_D21]] |
| 144 | + else |
| 145 | + R1 = try |
| 146 | + inv(α*I - s2_D22*s1_D22) # slightly faster than α*inv(I - α*s2_D22*s1_D22) |
| 147 | + catch |
| 148 | + error("Ill-posed feedback interconnection, I - α*s2_D22*s1_D22 or I - α*s2_D22*s1_D22 not invertible") |
| 149 | + end |
| 150 | + |
| 151 | + R2 = try |
| 152 | + inv(I - α*s1_D22*s2_D22) |
| 153 | + catch |
| 154 | + error("Ill-posed feedback interconnection, I - α*s2_D22*s1_D22 or I - α*s2_D22*s1_D22 not invertible") |
| 155 | + end |
| 156 | + |
| 157 | + A = [[sys1.A + s1_B2*R1*s2_D22*s1_C2 s1_B2*R1*s2_C2]; |
| 158 | + [s2_B2*R2*s1_C2 sys2.A + α*s2_B2*R2*s1_D22*s2_C2]] |
| 159 | + |
| 160 | + B = [[s1_B1 + s1_B2*R1*s2_D22*s1_D21 s1_B2*R1*s2_D21]; |
| 161 | + [s2_B2*R2*s1_D21 s2_B1 + α*s2_B2*R2*s1_D22*s2_D21]] |
| 162 | + C = [[s1_C1 + s1_D12*R1*s2_D22*s1_C2 s1_D12*R1*s2_C2]; |
| 163 | + [s2_D12*R2*s1_C2 s2_C1 + α*s2_D12*R2*s1_D22*s2_C2]] |
| 164 | + D = [[s1_D11 + s1_D12*R1*s2_D22*s1_D21 s1_D12*R1*s2_D21]; |
| 165 | + [s2_D12*R2*s1_D21 s2_D11 + α*s2_D12*R2*s1_D22*s2_D21]] |
| 166 | + end |
| 167 | + |
| 168 | + Dfinal = D[Zperm, Wperm] |
| 169 | + return HeteroStateSpace(A, B[:, Wperm], C[Zperm,:], Dfinal, timeevol) |
| 170 | + |
| 171 | +end |
| 172 | + |
| 173 | +function *(sys1::StaticStateSpace, sys2::StaticStateSpace) |
| 174 | + #Check dimension alignment |
| 175 | + #Note: sys1*sys2 = y <- sys1 <- sys2 <- u |
| 176 | + if (sys1.nu != sys2.ny) && (sys1.nu == 1 || sys2.ny == 1) |
| 177 | + throw(DimensionMismatch("sys1*sys2: sys1 must have same number of inputs as sys2 has outputs. If you want to broadcast a scalar system to a diagonal system, use broadcasted multiplication sys1 .* sys2")) |
| 178 | + end |
| 179 | + sys1.nu == sys2.ny || throw(DimensionMismatch("sys1*sys2: sys1 must have same number of inputs as sys2 has outputs")) |
| 180 | + timeevol = common_timeevol(sys1,sys2) |
| 181 | + T = promote_type(numeric_type(sys1), numeric_type(sys2)) |
| 182 | + |
| 183 | + O = @SMatrix zeros(size(sys2.A, 1), size(sys1.A, 2)) |
| 184 | + A = [[sys1.A sys1.B*sys2.C]; |
| 185 | + [O sys2.A]] |
| 186 | + B = [sys1.B*sys2.D ; sys2.B] |
| 187 | + C = [sys1.C sys1.D*sys2.C] |
| 188 | + D = sys1.D*sys2.D |
| 189 | + return HeteroStateSpace(A, B, C, D, timeevol) |
| 190 | +end |
| 191 | + |
| 192 | + |
| 193 | +function *(sys1::StaticStateSpace, sys2::AbstractStateSpace) |
| 194 | + sys1*StaticStateSpace(sys2) |
| 195 | +end |
| 196 | + |
| 197 | +function *(sys1::AbstractStateSpace, sys2::StaticStateSpace) |
| 198 | + StaticStateSpace(sys1)*sys2 |
| 199 | +end |
| 200 | + |
| 201 | + |
| 202 | + |
| 203 | +@autovec () function freqresp_nohess!(R::Array{T,3}, sys::StaticStateSpace, w_vec::AbstractVector{W}) where {T, W <: Real} |
| 204 | + ny, nu = size(sys) |
| 205 | + @boundscheck size(R) == (ny,nu,length(w_vec)) |
| 206 | + nx = sys.nx |
| 207 | + if nx == 0 # Only D-matrix |
| 208 | + @inbounds for i in eachindex(w_vec) |
| 209 | + R[:,:,i] .= sys.D |
| 210 | + end |
| 211 | + return R |
| 212 | + end |
| 213 | + A,B,C0,D = ssdata(sys) |
| 214 | + C = complex.(C0) # Still important when using ForwardDiff |
| 215 | + te = sys.timeevol |
| 216 | + |
| 217 | + let R=R, A=A, B=B, C=C, D=D, te=te |
| 218 | + @inbounds Polyester.@batch for i in eachindex(w_vec) |
| 219 | + Ri = @views R[:,:,i] |
| 220 | + copyto!(Ri,D) # start with the D-matrix |
| 221 | + isinf(w_vec[i]) && continue |
| 222 | + w = _freq(w_vec[i], te) |
| 223 | + Ac = A - w*I |
| 224 | + Bc = Ac \ B # Bc = (A - w*I)\B |
| 225 | + Ri .-= C*Bc # - rather than + since (A - w*I) instead of (w*I - A) |
| 226 | + end |
| 227 | + end |
| 228 | + R |
| 229 | +end |
0 commit comments