diff --git a/Project.toml b/Project.toml index 75fb9986..1f284e53 100644 --- a/Project.toml +++ b/Project.toml @@ -6,6 +6,7 @@ version = "0.1.0" [deps] ControlSystems = "a6e380b2-a6ca-5380-bf3e-84a91bcd477e" LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" +MonteCarloMeasurements = "0987c9cc-fe09-11e8-30f0-b96dd679fdca" Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" RecipesBase = "3cdcf5f2-1ef4-517c-9805-6587b60abb01" Statistics = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" diff --git a/src/RobustAndOptimalControl.jl b/src/RobustAndOptimalControl.jl index 2f580f4d..6a6ccd3a 100644 --- a/src/RobustAndOptimalControl.jl +++ b/src/RobustAndOptimalControl.jl @@ -4,6 +4,8 @@ using LinearAlgebra, Statistics using RecipesBase using ControlSystems import ControlSystems: ss, ssdata, ninputs, noutputs, nstates, isdiscrete, iscontinuous, to_matrix, timeevol, _string_mat_with_headers, PartionedStateSpace, common_timeevol +using MonteCarloMeasurements +using MonteCarloMeasurements.StaticArrays export ExtendedStateSpace @@ -18,6 +20,6 @@ include("hinfinity_design.jl") include("plotting.jl") include("reduction.jl") include("h2_design.jl") - +include("mcm.jl") end diff --git a/src/mcm.jl b/src/mcm.jl new file mode 100644 index 00000000..0c92bf77 --- /dev/null +++ b/src/mcm.jl @@ -0,0 +1,126 @@ +function sys2Rn(fun, H::AbstractStateSpace) + Ap,Bp,Cp,Dp = ssdata(H) + PTF = ControlSystems.numeric_type(H) + T,N,PT = MonteCarloMeasurements.particletypetuple(PTF) + A = getindex.(Ap, 1) + B = getindex.(Bp, 1) + C = getindex.(Cp, 1) + D = getindex.(Dp, 1) + te = H.timeevol + individuals = map(1:N) do i + A .= getindex.(Ap, i) + B .= getindex.(Bp, i) + C .= getindex.(Cp, i) + D .= getindex.(Dp, i) + sys = ss(A,B,C,D,te) + fun(sys) + end + MonteCarloMeasurements._finish_individuals(PT, Val(N), individuals, individuals[1]) + +end + + + +function sys2sys(fun, H::AbstractStateSpace; allow_static=true, static_th = 10) + Ap,Bp,Cp,Dp = ssdata(H) + PTF = ControlSystems.numeric_type(H) + ET,N,PT = MonteCarloMeasurements.particletypetuple(PTF) + nx, nu, ny = H.nx, H.nu, H.ny + if allow_static && size(Ap, 1) <= static_th + individuals_s = static_kernel(fun, H, ET, PT, Val(nx), Val(nu), Val(ny), Val(nx*nx), Val(nx*nu), Val(nx*ny), Val(nu*ny)) + # individuals_s = static_kernel(fun, H, ET, PT, Val{nx}(), Val{nu}(), Val{ny}(), Val{nx*nx}(), Val{nx*nu}(), Val{nx*ny}(), Val{nu*ny}()) + return mergesystems(PTF, individuals_s) + end + A = getindex.(Ap, 1) + B = getindex.(Bp, 1) + C = getindex.(Cp, 1) + D = getindex.(Dp, 1) + te = H.timeevol + individuals = map(1:N) do i + A .= getindex.(Ap, i) + B .= getindex.(Bp, i) + C .= getindex.(Cp, i) + D .= getindex.(Dp, i) + sys = ss(A,B,C,D,te) + fun(sys) + end + mergesystems(PTF, individuals) +end + +function static_kernel(fun::F, H, ::Type{ET}, ::Type{PT}, ::Val{nx}, ::Val{nu}, ::Val{ny}, ::Val{nxnx}, ::Val{nxnu}, ::Val{nxny}, ::Val{nynu}) where {F,ET,PT,nx,nu,ny,nxnx,nxnu,nxny,nynu} + Ap,Bp,Cp,Dp = ssdata(H) + te = H.timeevol + map(1:nparticles(Ap)) do pi + A = SMatrix{nx, nx, ET, nxnx}(ntuple(i->Ap[i][pi], nxnx)) + B = SMatrix{nx, nu, ET, nxnu}(ntuple(i->Bp[i][pi], nxnu)) + C = SMatrix{ny, nx, ET, nxny}(ntuple(i->Cp[i][pi], nxny)) + D = SMatrix{ny, nu, ET, nynu}(ntuple(i->Dp[i][pi], nynu)) + sys = HeteroStateSpace(A,B,C,D,te) + fun(sys) + end +end + +for PT in MonteCarloMeasurements.ParticleSymbols + + @eval function mergesystems(::Type{$PT{ET,N}},individuals::AbstractArray{<:StateSpace}) where {ET, N} + PTF = $PT{ET, N} # full particle type + s = individuals[1] + nx,nu,ny = s.nx, s.nu, s.ny + + A::Matrix{PTF} = [ + PTF([individuals[pi].A[i,j] for pi in 1:N]) + for i in 1:nx, j in 1:nx + ] + B::Matrix{PTF} = [ + PTF([individuals[pi].B[i,j] for pi in 1:N]) + for i in 1:nx, j in 1:nu + ] + C::Matrix{PTF} = [ + PTF([individuals[pi].C[i,j] for pi in 1:N]) + for i in 1:ny, j in 1:nx + ] + D::Matrix{PTF} = [ + PTF([individuals[pi].D[i,j] for pi in 1:N]) + for i in 1:ny, j in 1:nu + ] + + ss(A,B,C,D,s.timeevol) + + end + @eval mergesystems(::Type{$PT{ET,N}},t::Vector{<:Tuple}) where {ET, N} = ntuple(i->mergesystems($PT{ET,N}, getindex.(t, i)), length(t[1])) + @eval function mergesystems(::Type{$PT{ET,N}},a::Vector{<:AbstractArray}) where {ET, N} + PTF = $PT{ET, N} + nx = length(a[1]) + ret = [ + PTF([a[pi][i] for pi in 1:N]) + for i in 1:nx + ] + reshape(ret, size(a[1])) + end + # TODO: jag höll på med dessa, hade varit nice om tuple som returvärde hade funkat, försöker med att rekursivt kalla på mergesystems som då måste funka för arrays också +end + + + +# for f in (:pole, :tzero) +# @eval function ControlSystems.$f(s::TransferFunction{<:Any, <:ControlSystems.SisoRational{<:AbstractParticles}}) +# sys2Rn($f, s) +# end +# end + +for f in (:pole, :tzero) + @eval function ControlSystems.$f(s::StateSpace{<:Any, <:AbstractParticles}) + sys2Rn($f, s) + end +end + +for f in (:_preprocess_for_freqresp, :minreal, :balreal) + @eval function ControlSystems.$f(s::StateSpace{<:Any, <:AbstractParticles}, args...; kwargs...) + sys2sys(s->$f(s, args...; kwargs...), s, allow_static=false) + end +end + +# function ControlSystems.balreal(s::StateSpace{<:Any, <:AbstractParticles}, args...; kwargs...) +# sys2sys(s->balreal(s, args...; kwargs...)[1], s, allow_static=false) +# end + diff --git a/test/test_mcm.jl b/test/test_mcm.jl new file mode 100644 index 00000000..bc84d17c --- /dev/null +++ b/test/test_mcm.jl @@ -0,0 +1,56 @@ +using RobustAndOptimalControl +import RobustAndOptimalControl as RC +using MonteCarloMeasurements +using ControlSystems +unsafe_comparisons(false) +G = tf(1, [1 ± 0.1, 1]) + + + +funs = [ + G->c2d(G, 0.01) + pole + tzero + minreal + ss + bode + step + impulse + dcgain +] + +for fun in funs + @show fun + @test_nowarn fun(G) +end + +@test dcgain(G)[] == 1 +@test real(pole(G)[]) == -denvec(G)[][1] + + +H = ss(G) + +funs = [ + G->c2d(G, 0.01) + pole + tzero + minreal + balreal + tf + # bode + # step + # impulse + dcgain +] + + +for fun in funs + @show fun + @test_nowarn fun(H) +end + +RC.sys2Rn(pole, H) + + +w = exp10.(LinRange(-1, 1, 100)) +@test_nowarn bode(H, w) \ No newline at end of file