sort delta blocks

Author: baggepinnen

Project.toml

@@ -1,7 +1,7 @@
 name = "RobustAndOptimalControl"
 uuid = "21fd56a4-db03-40ee-82ee-a87907bee541"
 authors = ["Fredrik Bagge Carlson", "Marcus Greiff"]
-version = "0.2.0"
+version = "0.2.1"
 
 [deps]
 ComponentArrays = "b0b7db55-cfe3-40fc-9ded-d10e2dbeff66"

src/uncertainty_interface.jl

@@ -502,4 +502,139 @@ function IntervalArithmetic.Interval(d::δ{C}) where C <: Complex
     re = Interval(d.val.re-d.radius, d.val.re+d.radius)
     im = Interval(d.val.im-d.radius, d.val.im+d.radius)
     Complex(re, im)
-end
+end
+
+
+
+"""
+    block_structure(Δ)
+
+Take a vector of uncertain elements and return a vector of vectors with the block
+structure of perturbation blocks as described expected my μ-tools, i.e.
+- `[-N, 0]` denots a repeated real block of size `N`
+- `[N, 0]` denots a repeated complex block of size `N`
+- `[ny, nu]` denots a full complex block of size `ny × nu`
+"""
+function block_structure(D)
+    perm = sortperm(D, by=d->d.name)
+    D = D[perm]
+    blocks = Vector{Int}[]
+    blockstart = 1
+    for i in 2:length(D)
+        if D[i].name == D[i-1].name
+            if blockstart == 0
+                blockstart = i-1
+            end
+        else
+            push!(blocks, makeblock(D[blockstart:i-1]))
+            blockstart = i
+        end
+    end
+    push!(blocks, makeblock(D[blockstart:length(D)]))
+    blocks, perm
+end
+
+function makeblock(d) # TODO: full uncertain block that is not a dynamical system is not supported
+    N = length(d)
+    if N > 1 || (N==1 && d[] isa δ) # Δ = δI -> μ(M) = ρ(M) 8.80
+        if eltype(d[1]) <: Complex
+            b = [N, 0] # complex*I
+        else
+            b = [-N, 0] # real*I
+        end
+    else
+        b = [size(d[], 1), size(d[], 2)]
+    end
+end
+
+
+function blocksort(P::UncertainSS)
+    blocks, perm = block_structure(P.Δ)
+    Poutinds = []
+    Pininds = []
+    # Build indices into M that corresponds to the Δ blocks and permute those indices with the same permutation as was applied to Δ
+    for (i, d) in enumerate(P.Δ)
+        if i == 1
+            push!(Poutinds, 1:size(d, 2))
+            push!(Pininds, 1:size(d, 1))
+        else
+            push!(Poutinds, (1:size(d, 2)) .+ Poutinds[i-1][end])
+            push!(Pininds, (1:size(d, 1)) .+ Pininds[i-1][end])
+        end
+    end
+    Poutinds = reduce(vcat, Poutinds[perm])
+    Pininds = reduce(vcat, Pininds[perm])
+    M = P.M[Poutinds, Pininds]
+    blocks, M
+end
+
+function getinds(blocks::Vector{Vector{Int}}, d::Int)
+    lengths = [b[2] <= 1 ? 1 : b[d] for b in blocks]
+    endinds = cumsum(lengths)
+    startinds = [1; endinds[1:end-1] .+ 1]
+    inds = UnitRange.(0 .+ startinds, lengths .+ startinds .- 1)
+end
+
+# function getinds(D::AbstractVector, d::Int)
+#     lengths = size.(D, d)
+#     endinds = cumsum(lengths)
+#     startinds = [1; endinds[1:end-1] .+ 1]
+#     inds = UnitRange.(0 .+ startinds, lengths .+ startinds .- 1)
+# end
+
+# struct BlockDiag{T}
+#     D::Vector{T}
+#     rinds::Vector{UnitRange{Int}}
+#     cinds::Vector{UnitRange{Int}}
+# end
+# BlockDiag(D::AbstractVector) = BlockDiag(D, getinds(D, 1), getinds(D, 2))
+
+# Base.size(B::BlockDiag) = (B.rinds[end][end], B.cinds[end][end])
+
+# function Base.:*(A, B::BlockDiag)
+#     rinds = B.rinds
+#     submats = map(enumerate(rinds)) do (i,inds)
+#         A[:, inds] * B.D[i]
+#     end
+#     reduce(hcat, submats)
+# end
+
+# function quadform_minus(Q::BlockDiag, M, b, op)
+#     # con = M'Q*M - b^2*Q ⪯ I(n)
+#     rinds = Q.rinds
+#     cinds = Q.cinds
+#     map(eachindex(rinds)) do i
+#         r = rinds[i]
+#         c = cinds[i]
+#         q = Q.D[i]
+#         op(M[r,:]'q*M[c,:] .- b^2*q, I(size(M,2)))
+#     end
+# end
+
+
+
+#=
+D_from_Delta: I-full, full-I, diag-diag
+Version that constructs from initial guess
+=#
+
+
+# See Skogestad 8.8.3
+# for complex δ:
+# Δ = δI -> μ(M) = ρ(M) 8.80
+# Full block complex -> μ(M) = σ̄(M) = opnorm(M)
+# In general ρ(M) <= μ(M) <= σ̄(M)
+
+# function make_D(blocks)
+#     D = []
+#     for b in blocks
+#         if b[2] == 0
+#             n = abs(b[1])
+#             T = b[1] > 0 ? ComplexF64 : Float64
+#             push!(D, zeros(T, n, n))
+#         else
+#             push!(D, 0*I)
+#         end
+#     end
+#     BlockDiagonal(D)
+# end

test/test_uncertainty.jl

@@ -189,18 +189,75 @@ mu = ss(I(4)) + δss(4,4, bound=0.2)
 @test size(Pn) == size(convert(UncertainSS, Pn))
 @test convert(UncertainSS, Pn).D22 == Pn.D
 
-
-
 P = Pn*mu
-
-
 Pn2 = system_mapping(P)
 @test Pn2 == Pn
 @test size(P) == (7,8)
 @test size(P.M) == (4,4)
 
 
-##
+## this time mimo real
+delta = uss([δr(), δr()])
+a = 1
+P = ss([0 a; -a -1], I(2), [1 a; 0 1], 0)* (ss(1.0*I(2)) + delta)
+K = ss(1.0I(2))
+
+G = lft(P, -K)
+hn = norm(G, Inf)
+
+w = exp10.(LinRange(-5, 1, 100))
+M = freqresp(G.M, w)
+M = permutedims(M, (2,3,1))
+# mu = mussv_DG(M)
+# maximum(mu)
+# # maximum(structured_singular_value(M))
+# @test 1/maximum(mu) ≈ √(2) atol=0.01
+
+
+
+## Test block structure
+d = δr()
+@test RobustAndOptimalControl.makeblock([d]) == [-1, 0]
+@test RobustAndOptimalControl.makeblock([d,d]) == [-2, 0]
+
+d = δc()
+@test RobustAndOptimalControl.makeblock([d]) == [1,0]
+@test RobustAndOptimalControl.makeblock([d,d]) == [2, 0]
+@test RobustAndOptimalControl.makeblock(δss(2,3).Δ) == [2, 3]
+
+
+
+Δ = [δr(0,1,:qkarne), δr(0,1,:chuck_norris)]
+@test RobustAndOptimalControl.block_structure(Δ)[1] == [[-1, 0], [-1, 0]]
+
+Δ = [δr(0,1,:qkarne), δc(0,1,:chuck_norris)]
+@test RobustAndOptimalControl.block_structure(Δ)[1] == [[1, 0], [-1, 0]] # The two elements should have been sorted and 1,-1 are therefore switched
+
+
+Δ = [δr(0,1,:a), δss(2,3).Δ[], δr(0,1,:a), δc(0,1,:chuck_norris), δr(0,1,:a)]
+
+@test RobustAndOptimalControl.block_structure(Δ)[1] == [
+    [2, 3], # The gensym name will come first
+    [-3, 0],
+    [1, 0],
+]
+
+perm = RobustAndOptimalControl.block_structure(Δ)[2]
+@test perm == [2,1,3,5,4]
+
+P = partition(ssrand(8, 7, 2), 6, 7)
+P = UncertainSS(P, Δ)
+
+blocks, M = RobustAndOptimalControl.blocksort(P)
+@test blocks == [
+    [2, 3], # The gensym name will come first
+    [-3, 0],
+    [1, 0],
+]
+
+@test M[7,6] == P.M[6, 5] # test that the permutation was correctly applied, only test for the complex permutation
+@test M[1:3,1:2] == P.M[2:4, 2:3]# test that the permutation was correctly applied, only test for the matrix block
+
 w = exp10.(LinRange(-2, 2, 500))
 delta = δss(1,1)
 P = ss(tf(1,[1, .2, 1])) * (1+0.2*delta)
@@ -249,9 +306,5 @@ dm = diskmargin(system_mapping(P), K, 0, w)
 dmm = argmin(dm->dm.margin, dm.simultaneous_input)
 @test dmm.margin ≈ 0.5335 atol = 0.001
 
-## this time mimo real
-delta = uss([δr(), δr()])
-a = 1
-P = ss([0 a; -a -1], I(2), [1 a; 0 1], 0)* (ss(1.0*I(2)) + delta)
-K = ss(1.0I(2))
+