diff --git a/.gitignore b/.gitignore index bbcba11fa..c4cac0061 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ docs/build/ docs/site/ +.DS_Store diff --git a/docs/make.jl b/docs/make.jl index 8fb504aaf..07476d8d3 100644 --- a/docs/make.jl +++ b/docs/make.jl @@ -1,4 +1,4 @@ -using Documenter, ControlSystems, Plots, LinearAlgebra, DSP +using Documenter, ControlSystems, Plots import GR # Bug with world age in Plots.jl, see https://github.com/JuliaPlots/Plots.jl/issues/1047 include("src/makeplots.jl") @@ -23,6 +23,6 @@ end deploydocs( repo = "github.com/JuliaControl/ControlSystems.jl.git", latest = "master", - julia = "1.0", + julia = "0.6", deps = myDeps ) diff --git a/docs/src/makeplots.jl b/docs/src/makeplots.jl index 1c0138bdc..aea50366a 100644 --- a/docs/src/makeplots.jl +++ b/docs/src/makeplots.jl @@ -16,7 +16,7 @@ Q = Matrix{Float64}(I,2,2) R = Matrix{Float64}(I,1,1) L = dlqr(A,B,Q,R) # lqr(sys,Q,R) can also be used -u(x,t) = -L*x .+ 1.5(t>=2.5)# Form control law (u is a function of t and x), a constant input disturbance is affecting the system from t≧2.5 +u(x,t) = -L*x + 1.5(t>=2.5)# Form control law (u is a function of t and x), a constant input disturbance is affecting the system from t≧2.5 t=0:h:5 x0 = [1,0] y, t, x, uout = lsim(sys,u,t,x0=x0) @@ -71,7 +71,7 @@ Plots.savefig(plotsDir*"/ppgofplot.svg") P1(s) = exp(-sqrt(s)) f1 = stabregionPID(P1,exp10.(range(-5, stop=1, length=1000))); Plots.savefig(plotsDir*"/stab1.svg") -P2 = s -> 100*(s+6).^2 ./(s.*(s+1).^2 .*(s+50).^2) +P2 = s -> 100*(s+6).^2./(s.*(s+1).^2.*(s+50).^2) f2 = stabregionPID(P2,exp10.(range(-5, stop=2, length=1000))); Plots.savefig(plotsDir*"/stab2.svg") P3 = tf(1,[1,1])^4 f3 = stabregionPID(P3,exp10.(range(-5, stop=0, length=1000))); Plots.savefig(plotsDir*"/stab3.svg") @@ -84,7 +84,7 @@ f3 = stabregionPID(P3,exp10.(range(-5, stop=0, length=1000))); Plots.savefig(plo P = tf([1.],[1., 1]) ζ = 0.5 # Desired damping ws = exp10.(range(-1, stop=2, length=8)) # A vector of closed-loop bandwidths -kp = 2*ζ*ws.-1 # Simple pole placement with PI given the closed-loop bandwidth, the poles are placed in a butterworth pattern +kp = 2*ζ*ws-1 # Simple pole placement with PI given the closed-loop bandwidth, the poles are placed in a butterworth pattern ki = ws.^2 pidplots(P,:nyquist,;kps=kp,kis=ki, ω= exp10.(range(-2, stop=2, length=500))) Plots.savefig(plotsDir*"/pidplotsnyquist1.svg") @@ -92,7 +92,7 @@ pidplots(P,:gof,;kps=kp,kis=ki, ω= exp10.(range(-2, stop=2, length=500))) Plots.savefig(plotsDir*"/pidplotgof1.svg") kp = range(-1, stop=1, length=8) # Now try a different strategy, where we have specified a gain crossover frequency of 0.1 rad/s -ki = sqrt.(1 .-kp.^2)./10 +ki = sqrt.(1-kp.^2)./10 pidplots(P,:nyquist,;kps=kp,kis=ki) Plots.savefig(plotsDir*"/pidplotsnyquist2.svg") pidplots(P,:gof,;kps=kp,kis=ki) diff --git a/example/hinf_example_DC.jl b/example/hinf_example_DC.jl new file mode 100644 index 000000000..92cda95e6 --- /dev/null +++ b/example/hinf_example_DC.jl @@ -0,0 +1,64 @@ +using Plots +using ControlSystems +""" +This is a simple SISO example with a pole in the origin, corresponding to the +DC servos used in the Lund laboratories. It serves to exeplify how the syntheis +can be done for simple SISO systems, and also demonstrates how we chan verify +if the problem is feasible to solve using the ARE method. + +The example can be set to visualize and save plots using the variables + MakePlots - true/false (true if plots are to be generated, false for testing) + SavePlots - true/false (true if plots are to be saved, false for testing) +""" +MakePlots, SavePlots = false, false + +# Define the process +Gtrue = tf([11.2], [1, 0.12,0]) + +# Sensitivity weight function +M, wB, A = 1.5, 20.0, 1e-8 +WS = tf([1/M, wB],[1, wB*A]) + +# Output sensitivity weight function +WU = ss(1) + +# Complementary sensitivity weight function +WT = [] + +# Form the P in the LFT F_l(P,C) as a partitioned state-space object +P = hinfpartition(Gtrue, WS, WU, WT) + +# Check if the system is feasible for synythesis +flag = hinfassumptions(P) + +# Since it is not, modify the plant desciption +epsilon = 1e-5 +G = tf([11.2], [1, 0.12]) * tf([1], [1, epsilon]) + +# Form the P in the LFT Fl(P,C) as a partitioned state-space object +P = hinfpartition(G, WS, WU, WT) + +# Check if the problem is feasible +flag = hinfassumptions(P) + +# Synthesize the H-infinity optimal controller +flag, C, gamma = hinfsynthesize(P) + +# Extract the transfer functions defining some signals of interest +Pcl, S, CS, T = hinfsignals(P, G, C) + +## Plot the specifications +if MakePlots + specificationplot([S, CS, T], [WS, WU, WT], gamma) + if SavePlots + savefig("example_DC_specifications.pdf") + end +end + +## Plot the closed loop gain from w to z +if MakePlots + specificationplot(Pcl, gamma; s_labels=["\$\\sigma(P_{cl}(j\\omega))\$"], w_labels=["\$\\gamma\$"]) + if SavePlots + savefig("example_DC_clgain.pdf") + end +end diff --git a/example/hinf_example_DC_discrete.jl b/example/hinf_example_DC_discrete.jl new file mode 100644 index 000000000..d8b29da49 --- /dev/null +++ b/example/hinf_example_DC_discrete.jl @@ -0,0 +1,60 @@ +using Plots +using ControlSystems +""" +This is a simple SISO example with a pole in the origin, corresponding to the +DC servos used in the Lund laboratories. It serves to exeplify how the syntheis +can be done for simple SISO systems, and also demonstrates how we chan verify +if the problem is feasible to solve using the ARE method. + +The example can be set to visualize and save plots using the variables + MakePlots - true/false (true if plots are to be generated, false for testing) + SavePlots - true/false (true if plots are to be saved, false for testing) +""" +MakePlots = true + +# Define the process +ts = 0.01 +epsilon = 1e-5 +Gd = ss(c2d(tf([11.2], [1, 0.12]) * tf([1], [1, epsilon]), ts)) + +# Sensitivity weight function +M, wB, A = 1.5, 20.0, 1e-8 +WS = tf([1/M, wB],[1, wB*A]) + +# Output sensitivity weight function +WU = ss(1) + +# Complementary sensitivity weight function +WT = [] + +# Create continuous time approximation of the process +Gc = bilineard2c(ss(Gd)) + +# Form the P in the LFT Fl(P,C) as a partitioned state-space object +Pc = hinfpartition(Gc, WS, WU, WT) + +# Check if the problem is feasible +flag = hinfassumptions(Pc) + +# Synthesize the H-infinity optimal controller +flag, Cc, gamma = hinfsynthesize(Pc) + +# Extract the transfer functions defining some signals of interest, but do so +# using discrete equivalent of the continuous time objects Pc, Cc and Gc +PclD, SD, CSD, TD = hinfsignals( + bilinearc2d(Pc, ts), + bilinearc2d(Gc, ts), + bilinearc2d(Cc, ts) +) + +# This solution is a bit hacky and should be revised +Pcl = ss(PclD.A, PclD.B, PclD.C, PclD.D, ts) +S = ss(SD.A, SD.B, SD.C, SD.D, ts) +CS = ss(CSD.A, CSD.B, CSD.C, CSD.D, ts) +T = ss(TD.A, TD.B, TD.C, TD.D, ts) + +# Visualize results +if MakePlots + specificationplot([S, CS, T], [WS, WU, WT], gamma) + specificationplot(Pcl, gamma; s_labels=["\$\\sigma(P_{cl}(j\\omega))\$"], w_labels=["\$\\gamma\$"]) +end diff --git a/example/hinf_example_MIT.jl b/example/hinf_example_MIT.jl new file mode 100644 index 000000000..ab9df2f9c --- /dev/null +++ b/example/hinf_example_MIT.jl @@ -0,0 +1,58 @@ +using ControlSystems +using Plots + +""" +This is a simple SISO example which was used for debugging the implementation, +as this exact example was use in the lecture notes of the "Principles of Optimal +Control" cours of the MIT OpenCourseWare [1], where the choice of weighting +functions and dynamics gave rise to an H-infinity optimal cotnroller with a +γ of approximately 1.36, where, in our case, we get a controller at 0.93 + +[1] https://ocw.mit.edu/courses/aeronautics-and-astronautics/16-323-principles-of-optimal-control-spring-2008/lecture-notes/lec15.pdf + +The example can be set to visualize and save plots using the two variables + ShowPlots - true/false (true if plots are to be generated, false for testing) + filename - Set to string if files are to be saved, otherwise set a empty list +""" +MakePlots, SavePlots = false, false + +# Define the process +G = tf([200], [0.025,1.0025,10.1,1]) + +# Sensitivity weight function +M, wB, A = 1.5, 10, 1e-4 +WS = tf([1/M, wB],[1, wB*A]) + +# Output sensitivity weight function +WU = ss(0.1) + +# Complementary sensitivity weight function +WT = [] + +# Form augmented P dynamics in state-space +P = hinfpartition(G, WS, WU, WT) + +# Check that the assumptions are satisfied +flag = hinfassumptions(P) + +# Synthesize the H-infinity optimal controller +flag, C, gamma = hinfsynthesize(P) + +# Extract the transfer functions defining some signals of interest +Pcl, S, CS, T = hinfsignals(P, G, C) + +## Plot the specifications +if MakePlots + specificationplot([S, CS, T], [ss(WS), WU, WT], gamma) + if SavePlots + savefig("example_MIT_specifications.pdf") + end +end + +## Plot the closed loop gain from w to z +if MakePlots + specificationplot(Pcl, gamma; s_labels=["\$\\sigma(P_{cl}(j\\omega))\$"], w_labels=["\$\\gamma\$"]) + if SavePlots + savefig("example_MIT_clgain.pdf") + end +end diff --git a/example/hinf_example_MIT_discrete.jl b/example/hinf_example_MIT_discrete.jl new file mode 100644 index 000000000..2d0421753 --- /dev/null +++ b/example/hinf_example_MIT_discrete.jl @@ -0,0 +1,60 @@ +using ControlSystems +using Plots + +""" +This is a simple SISO example which was used for debugging the implementation, +as this exact example was use in the lecture notes of the "Principles of Optimal +Control" cours of the MIT OpenCourseWare [1], however, this example is in discrete time + +[1] https://ocw.mit.edu/courses/aeronautics-and-astronautics/16-323-principles-of-optimal-control-spring-2008/lecture-notes/lec15.pdf + +The example can be set to visualize and save plots using the two variables + MakePlots - true/false (true if plots are to be generated, false for testing) + SavePlots - true/false (true if plots are to be saved, false for testing) +""" +MakePlots = true + +# Define the process +ts = 0.005 +Gd = hInf_bilinear_s2z(ss(tf([200], [0.025,1.0025,10.1,1])),ts) + +# Sensitivity weight function +M, wB, A = 1.5, 10, 1e-4 +WS = tf([1/M, wB],[1, wB*A]) + +# Output sensitivity weight function +WU = ss(0.1) + +# Complementary sensitivity weight function +WT = [] + +# Create continuous time approximation of the process +Gc = bilineard2c(ss(Gd)) + +# Form augmented P dynamics in state-space +Pc = hinfpartition(Gc, WS, WU, WT) + +# Check that the assumptions are satisfied +flag = hinfassumptions(Pc) + +# Synthesize the H-infinity optimal controller +flag, Cc, gamma = hinfsynthesize(Pc) + +# Extract the transfer functions defining some signals of interest, but do so +# using discrete equivalent of the continuous time objects Pc, Cc and Gc +PclD, SD, CSD, TD = hinfsignals( + bilinearc2d(Pc, ts), + bilinearc2d(Gc, ts), + bilinearc2d(Cc, ts) +) + +Pcl = ss(PclD.A, PclD.B, PclD.C, PclD.D, ts) +S = ss(SD.A, SD.B, SD.C, SD.D, ts) +CS = ss(CSD.A, CSD.B, CSD.C, CSD.D, ts) +T = ss(TD.A, TD.B, TD.C, TD.D, ts) + +# Visualize results +if MakePlots + specificationplot([S, CS, T], [WS, WU, WT], gamma) + specificationplot(Pcl, gamma; s_labels=["\$\\sigma(P_{cl}(j\\omega))\$"], w_labels=["\$\\gamma\$"]) +end diff --git a/example/hinf_example_tank.jl b/example/hinf_example_tank.jl new file mode 100644 index 000000000..1152bb602 --- /dev/null +++ b/example/hinf_example_tank.jl @@ -0,0 +1,99 @@ +using ControlSystems +using Plots +using LinearAlgebra +""" +This is a simple SISO example with integrator dynamics corresponding to the +quad tank process in the lab. + +The example can be set to visualize and save plots using the variables + MakePlots - true/false (true if plots are to be generated, false for testing) + SavePlots - true/false (true if plots are to be saved, false for testing) +""" +MakePlots, SavePlots = false, false + +# Define the proces parameters +k1, k2, kc, g = 3.33, 3.35, 0.5, 981 +A1, A3, A2, A4 = 28, 28, 32, 32 +a1, a3, a2, a4= 0.071, 0.071, 0.057, 0.057 +h01, h02, h03, h04 = 12.4, 12.7, 1.8, 1.4 +T1, T2 = (A1/a1)*sqrt(2*h01/g), (A2/a2)*sqrt(2*h02/g) +T3, T4 = (A3/a3)*sqrt(2*h03/g), (A4/a4)*sqrt(2*h04/g) +c1, c2 = (T1*k1*kc/A1), (T2*k2*kc/A2) +gamma1, gamma2 = 0.7, 0.6 + +# Define the process dynamics +A = [-1/T1 0 A3/(A1*T3) 0; + 0 -1/T2 0 A4/(A2*T4); + 0 0 -1/T3 0; + 0 0 0 -1/T4]; +B = [gamma1*k1/A1 0; + 0 gamma2*k2/A2; + 0 (1-gamma2)*k2/A3; + (1-gamma1)*k1/A4 0 ]; +C = [kc 0 0 0; + 0 kc 0 0]; +D = zeros(2,2) +G = ss(A,B,C,D); + +# Sensitivity weight function +WSelement = 100*tf([1,1],[1000,1]) +WS = [WSelement 0; 0 WSelement] +iWSelement = 1/WSelement +iWS = [iWSelement 0; 0 iWSelement] + +# Output sensitivity weight function +WUelement = 5*tf([1,1],[0.1,1]) +WUelement = ss(0.01) +WU = [WUelement 0; 0 WUelement] +iWUelement = 1/WUelement +iWU = [iWUelement 0; 0 iWUelement] + +# Complementary sensitivity weight function +WTelement = tf([10,0.1],[1,1]) +WT = [WTelement 0; 0 WTelement] +iWTelement = 1/WTelement +iWT = [iWTelement 0; 0 iWTelement] + +# Form augmented P dynamics in state-space +P = hinfpartition(G, WS, WU, WT) + +# Check that the assumptions are satisfied +flag = hinfassumptions(P) + +# Synthesize the H-infinity optimal controller +flag, C, gamma = hinfsynthesize(P) + +Pcl, S, CS, T = hinfsignals(P, G, C) + +## Plot the specifications +# TODO figure out why I get segmentation errors when using ss instead of tf for +# the weighting functions, makes no sense at all +if MakePlots + specificationplot([S, CS, T], [WSelement, 0.01, WTelement], gamma) + if SavePlots + savefig("example_tank_specifications.pdf") + end +end + +## Plot the closed loop gain from w to z +# TODO figure out why the legends don't seem to work in this case +if MakePlots + specificationplot(Pcl, gamma; s_labels=["\$\\sigma(P_{cl}(j\\omega))\$"], w_labels=["\$\\gamma\$"]) + if SavePlots + savefig("example_tank_clgain.pdf") + end +end + +if MakePlots + times = [i for i in range(0, stop=300, length=10000)] + stepy, stept, stepx = step(T, times) + pStep1=plot(stept, stepy[:,1,1], color = :blue, w=2, label="\$u_1\\rightarrow y_1\$") + pStep2=plot(stept, stepy[:,1,2], color = :blue, w=2, label="\$u_1\\rightarrow y_2\$", ylims = (-0.5,1.1)) + pStep3=plot(stept, stepy[:,2,1], color = :blue, w=2, label="\$u_2\\rightarrow y_1\$", ylims = (-0.5,1.1)) + pStep4=plot(stept, stepy[:,2,2], color = :blue, w=2, label="\$u_2\\rightarrow y_2\$") + l = @layout [ a b c d ] + plt=plot(pStep1, pStep2, pStep3, pStep4, layout=l, size=(1000,250)) + if SavePlots + savefig("example_tank_stepresponse.pdf") + end +end diff --git a/example/hinf_example_tank_discrete.jl b/example/hinf_example_tank_discrete.jl new file mode 100644 index 000000000..12aa0fb6e --- /dev/null +++ b/example/hinf_example_tank_discrete.jl @@ -0,0 +1,96 @@ +using ControlSystems +using Plots +using LinearAlgebra +""" +This is a simple SISO example with integrator dynamics corresponding to the +quad tank process in the lab. + +The example can be set to visualize and save plots using the variables + MakePlots - true/false (true if plots are to be generated, false for testing) + SavePlots - true/false (true if plots are to be saved, false for testing) +""" +MakePlots = false +SavePlots = false + +# Define the proces parameters +k1, k2, kc, g = 3.33, 3.35, 0.5, 981 +A1, A3, A2, A4 = 28, 28, 32, 32 +a1, a3, a2, a4= 0.071, 0.071, 0.057, 0.057 +h01, h02, h03, h04 = 12.4, 12.7, 1.8, 1.4 +T1, T2 = (A1/a1)*sqrt(2*h01/g), (A2/a2)*sqrt(2*h02/g) +T3, T4 = (A3/a3)*sqrt(2*h03/g), (A4/a4)*sqrt(2*h04/g) +c1, c2 = (T1*k1*kc/A1), (T2*k2*kc/A2) +gamma1, gamma2 = 0.7, 0.6 + +# Define the process dynamics +A = [-1/T1 0 A3/(A1*T3) 0; + 0 -1/T2 0 A4/(A2*T4); + 0 0 -1/T3 0; + 0 0 0 -1/T4]; +B = [gamma1*k1/A1 0; + 0 gamma2*k2/A2; + 0 (1-gamma2)*k2/A3; + (1-gamma1)*k1/A4 0 ]; +C = [kc 0 0 0; + 0 kc 0 0]; +D = zeros(2,2) +ts = 0.01 +Gd = bilinearc2d(ss(A,B,C,D), ts) + +# Sensitivity weight function +WSelement = 100*tf([0.1,1],[1000,1]) +WS = [WSelement 0; 0 WSelement] +iWSelement = 1/WSelement +iWS = [iWSelement 0; 0 iWSelement] + +# Output sensitivity weight function +WUelement = 100*tf([1,1],[0.1,1]) ## +WUelement = ss(0.1) +WU = [WUelement 0; 0 WUelement] +iWUelement = 1/WUelement +iWU = [iWUelement 0; 0 iWUelement] + +# Complementary sensitivity weight function +WTelement = tf([10,0.1],[1,1]) +WT = [WTelement 0; 0 WTelement] +iWTelement = 1/WTelement +iWT = [iWTelement 0; 0 iWTelement] + +# Create continuous time approximation of the process +Gc = bilineard2c(Gd) +#Gc = ss(A,B,C,D) + +# Form the P in the LFT Fl(P,C) as a partitioned state-space object +Pc = hinfpartition(Gc, WS, WU, WT) + +# Check if the problem is feasible +flag = hinfassumptions(Pc) + +# Synthesize the H-infinity optimal controller +flag, Cc, gamma = hinfsynthesize(Pc) + +# Extract the transfer functions defining some signals of interest +Pcl, S, CS, T = hinfsignals(Pc, Gc, Cc) + +Pcl = bilinearc2d(Pcl, ts) +S = bilinearc2d(S, ts) +CS = bilinearc2d(CS, ts) +T = bilinearc2d(T, ts) + +if MakePlots + # Specifications + specificationplot([S, CS, T], [WSelement, 0.1, WTelement], gamma) + + # Closed-loop H-infinity norm + specificationplot(Pcl, gamma; s_labels=["\$\\sigma(P_{cl}(j\\omega))\$"], w_labels=["\$\\gamma\$"]) + + # Stepresponse + times = 0:ts:300 + stepy, stept, stepx = step(T, times) + pStep1=plot(stept, stepy[:,1,1], color = :blue, w=2, label="\$u_1\\rightarrow y_1\$") + pStep2=plot(stept, stepy[:,1,2], color = :blue, w=2, label="\$u_1\\rightarrow y_2\$", ylims = (-0.5,1.1)) + pStep3=plot(stept, stepy[:,2,1], color = :blue, w=2, label="\$u_2\\rightarrow y_1\$", ylims = (-0.5,1.1)) + pStep4=plot(stept, stepy[:,2,2], color = :blue, w=2, label="\$u_2\\rightarrow y_2\$") + l = @layout [ a b c d ] + plt=plot(pStep1, pStep2, pStep3, pStep4, layout=l, size=(1000,250)) +end diff --git a/src/ControlSystems.jl b/src/ControlSystems.jl index 69f07b137..2a5d3d7e7 100644 --- a/src/ControlSystems.jl +++ b/src/ControlSystems.jl @@ -2,6 +2,7 @@ module ControlSystems export LTISystem, StateSpace, + ExtendedStatespace, TransferFunction, ss, tf, @@ -71,8 +72,14 @@ export LTISystem, numvec, denvec, numpoly, - denpoly - + denpoly, + # Hinfinity design + hinfsynthesize, + hinfassumptions, + hinfpartition, + hinfsignals, + bilinearc2d, + bilineard2c # QUESTION: are these used? LaTeXStrings, Requires, IterTools using Polynomials, OrdinaryDiffEq, Plots, LaTeXStrings, LinearAlgebra @@ -98,6 +105,7 @@ include("types/SisoTfTypes/promotion.jl") include("types/SisoTfTypes/conversion.jl") include("types/StateSpace.jl") +include("types/ExtendedStatespace.jl") # Convenience constructors include("types/tf.jl") @@ -128,6 +136,8 @@ include("pid_design.jl") include("plotting.jl") +include("hinfinity_design.jl") + @deprecate num numvec @deprecate den denvec diff --git a/src/hinfinity_design.jl b/src/hinfinity_design.jl new file mode 100644 index 000000000..65036acb9 --- /dev/null +++ b/src/hinfinity_design.jl @@ -0,0 +1,889 @@ +""" The implementation is primarily based on a paper by Glover and Doyle, and +the code is best read with this docment at hand. All references to equations +in docstrings and comments are to the version of the paper given below: + + @article{glover1988state, + title={State-space formulae for all stabilizing controllers that satisfy an + H-infinity norm bound and relations to relations to risk sensitivity}, + author={Glover, Keith and Doyle, John C}, + journal={Systems & control letters}, + volume={11}, + number={3}, + pages={167--172}, + year={1988}, + publisher={Citeseer} + } +""" + + +"""`[flag] = function hinfassumptions(P::ExtendedStateSpace; verbose=true)` + +Check the assumptions for using the γ-iteration synthesis in Theorem 1. In +future revisions, we could suggest possible changes to P should the system not +be feasible for synthesis. However, this has not been too successful so far..! +""" +function hinfassumptions(P::ExtendedStateSpace; verbose=true) + + A, B1, B2, C1, C2, D11, D12, D21, D22 = ssdata(P) + + # Check assumption A1 + if !_stabilizable(A, B2) + if verbose + println("Warning, the system A is not stabilizable through B2, ", + "violation of assumption A1.") + end + return false + end + if !_detectable(A, C2) + if verbose + println("Warning, the system A is not detectable through C2, ", + "violation of assumption A1.") + end + return false + end + + # Check assumption A2 + if rank(D12) < size(D12,2) + if verbose + println("Warning, the matrix D12 does not have full rank, ", + "violation of assumption A2.") + end + return false + end + if rank(D21) < size(D21,1) + if verbose + println("Warning, the matrix D21 does not have full rank, ", + "violation of assumption A2.") + end + return false + end + + # Check assumption A5 + if rank(A - B2*pinv(D12)*C1) < size(A,1) + if verbose + println("Warning, the matrix (A - B2*D12^-*C1) does not have full", + "rank, violation of assumption A5.") + end + return false + end + # Check assumption A6 + if rank(A - B1*pinv(D21)*C2) < size(A,1) + if verbose + println("Warning, the matrix (A - B1*D21Pinv*C2) does not ", + "have full rank, violation of assumption A6.") + end + return false + end + + # All assumptions have passed, and we may proceed with the synthesis + if verbose println("All assumtions are satisfied!") end + return true +end + +"""`[flag] = _stabilizable(A::AbstractMatrix, B::AbstractMatrix)` + +Applies the Hautus lemma to check if the pair is stabilizable +""" +function _stabilizable(A::AbstractMatrix, B::AbstractMatrix) + eigValsA = eigvals(A) + for ii = 1:length(eigValsA) + if real(eigValsA[ii])>= 0 + if rank([eigValsA[ii] * Matrix{Float64}(I, size(A,1), size(A,2)) - A B]) != size(A,1) + return false + end + end + end + return true +end + +"""`[flag] = _detectable(A::AbstractMatrix, C::AbstractMatrix)` + +Applies the Hautus lemma to check if the pair is detectable +""" +function _detectable(A::AbstractMatrix, C::AbstractMatrix) + eigValsA = eigvals(A) + for ii = 1:length(eigValsA) + if real(eigValsA[ii])>= 0 + if rank([eigValsA[ii] * Matrix{Float64}(I, size(A,1), size(A,2)) - A; C]) != size(A,1) + return false + end + end + end + return true +end + +"""`[flag, K, gamma] = hinfsynthesize(P::ExtendedStateSpace; maxIter=20, interval=(2/3,20), verbose=true)` + +Computes an H-infinity optimal controller K for an extended plant P such that +||F_l(P, K)||∞ < γ for the largest possible gamma given P. The routine is +known as the γ-iteration, and is based on the paper "State-space formulae for +all stabilizing controllers that satisfy an H∞-norm bound and relations to +risk sensitivity" by Glover and Doyle. See the Bib-entry below [1] above. +""" +function hinfsynthesize(P::ExtendedStateSpace; maxIter=20, interval=(2/3,20), verbose=true, tolerance=1e-10) + + # Transform the system into a suitable form + Pbar, Ltrans12, Rtrans12, Ltrans21, Rtrans21 = _transformp2pbar(P) + + # Run the gamma iterations + XinfFeasible, YinfFeasible, FinfFeasible, HinfFeasible, gammFeasible = _gammaiterations(Pbar, maxIter, interval, verbose, tolerance) + + if !isempty(gammFeasible) + # Synthesize the controller and trnasform it back into the original coordinates + Ac, Bc, Cc, Dc = _synthesizecontroller(Pbar, XinfFeasible, YinfFeasible, FinfFeasible, HinfFeasible, gammFeasible, Ltrans12, Rtrans12, Ltrans21, Rtrans21) + + # Return the controller, the optimal gain γ, and a true flag + C = ss(Ac, Bc, Cc, Dc) + gamma = gammFeasible + flag = true + else + # Return and empty controller, empty gain γ, and a false flag + C = [] + gamma = [] + flag = false + end + return flag, C, gammFeasible +end + +"""`[Ac, Bc Cc, Dc] = _synthesizecontroller(P::ExtendedStateSpace, Xinf, Yinf, F, H, gamma, Ltrans12, Rtrans12, Ltrans21, Rtrans21)` + +Syntheize a controller by operating on the scaled state-space description of the +system (i.e., the state-space realization of Pbar) using the solutions from the +γ-iterations. The controller is synthesized in the coordinates of Pbar, and then +transformed back to the original coordinates by the linear transformations +Ltrans12, Rtrans12, Ltrans21 and Rtrans21. +""" +function _synthesizecontroller(P::ExtendedStateSpace, Xinf::AbstractMatrix, Yinf::AbstractMatrix, F::AbstractMatrix, H::AbstractMatrix, gamma::Number, Ltrans12::AbstractMatrix, Rtrans12::AbstractMatrix, Ltrans21::AbstractMatrix, Rtrans21::AbstractMatrix) + + A = P.A + B1 = P.B1 + B2 = P.B2 + C1 = P.C1 + C2 = P.C2 + D11 = P.D11 + D12 = P.D12 + D21 = P.D21 + D22 = P.D22 + + gSq = gamma * gamma + + B = [B1 B2] + C = [C1; C2] + + # Dimensionality + P1 = size(C1,1); + P2 = size(C2,1); + M1 = size(B1,2); + M2 = size(B2,2); + + # Equation (11) + F11=F[1:(M1-P2),:] + F12=F[(M1-P2+1):M1,:] + F2 =F[(M1+1):(M1+M2),:] + + # Equation (12) + H11=H[:,1:(P1-M2)] + H12=H[:,(P1-M2+1):P1] + H2 =H[:,(P1+1):(P1+P2)] + + # Definition of D in the assumptions section + D1111=D11[1:(P1-M2),1:(M1-P2)] + D1112=D11[1:(P1-M2),(M1-P2+1):M1] + D1121=D11[(P1-M2+1):P1,1:(M1-P2)] + D1122=D11[(P1-M2+1):P1,(M1-P2+1):M1] + + # Equation 19 + D11hat = ((-D1121 * D1111') / (gSq * I - D1111 * D1111')) * D1112 - D1122 + + # Equation 20 + D12hatD12hat = I - (D1121 / (gSq * I - D1111' * D1111)) * D1121' + _assertrealandpsd(D12hatD12hat; msg=" in equation (20)") + D12hat = cholesky(D12hatD12hat).L + + # Equation 21 + D21hatD21hat = I - (D1112' / (gSq * I - D1111 * D1111')) * D1112 + _assertrealandpsd(D21hatD21hat; msg=" in equation (21)") + D21hat = cholesky(D21hatD21hat).U + + # Equation 27 + Zinv = (I - Yinf * Xinf / gSq) + + # Equation 22 + B2hat = (B2 + H12) * D12hat + + # Equation 23 (using the inverse of 27) + C2hat = -D21hat * (C2 + F12) / Zinv + + # Equation 24 + B1hat = -H2 + (B2hat / D12hat) * D11hat + + # Equation 25 (using the inverse of 27) + C1hat = F2 / Zinv + (D11hat / D21hat) * C2hat + + # Equation 26 + Ahat = A + H * C + (B2hat / D12hat) * C1hat + + Acontroller = Ahat + + B1controller = B1hat * Ltrans21 + B2controller = B2hat + + C1controller = Rtrans12 * C1hat + C2controller = C2hat + + Bcontroller = [B1controller B2controller] + Ccontroller = [C1controller; C2controller] + + D11controller = Rtrans12 * D11hat * Ltrans21 + D12controller = D12hat * Ltrans21 + D21controller = Rtrans12 * D21hat + D22controller = zeros(size(D11hat)) + Dcontroller = [D11controller D12controller; D21controller D22controller] + + # TODO implement loop shift for any system hot satisfying A4 + + return Acontroller, Bcontroller[:,1:P2], Ccontroller[1:M2,:], Dcontroller[1:M2,1:P2] +end + +"""_assertrealandpsd(A::AbstractMatrix, msg::AbstractString) + +Check that a matrix is real and PSD - throw an error otherwise. +""" +function _assertrealandpsd(A::AbstractMatrix; msg="") + if any(real(eigvals(A)).<=0) + error(ErrorException(string("The matrix", msg," is not PSD."))) + end + if any(imag(eigvals(A)).!=0) + error(ErrorException(string("The matrix", msg," is not real."))) + end +end + +"""`[flag] = _checkfeasibility(Xinf, Yinf, gamma, tolerance, iteration; verbose=true)` + +Check the feasibility of the computed solutions Xinf, Yinf and the algebraic +Riccatti equations, return true if the solution is valid, and false otherwise. +""" +function _checkfeasibility(Xinf::AbstractMatrix, Yinf::AbstractMatrix, gamma::Number, tolerance::Number, iteration::Number; verbose=true) + + minXev = minimum(real(eigvals(Xinf))) + minYev = minimum(real(eigvals(Yinf))) + specrad = maximum(abs.(eigvals(Xinf*Yinf))) / (gamma*gamma) + + if verbose + if iteration == 1 + println("iteration, gamma") + end + println(iteration, " ", gamma) + end + + if minXev < -tolerance + # Failed test, eigenvalues of Xinf must be positive real + return false + end + if minYev < -tolerance + # Failed test, eigenvalues of Yinf must be positive real + return false + end + if specrad > 1 + # Failed test, spectral radius of XY must be greater than gamma squared + return false + end + return true +end + +"""`[solution] = _solvehamiltonianare(H)` + +Solves a hamiltonian Alebraic Riccati equation using the Schur-decomposition, +for additional details, see + + @article{laub1979schur, + title={A Schur method for solving algebraic Riccati equations}, + author={Laub, Alan}, + journal={IEEE Transactions on automatic control}, + volume={24}, + number={6}, + pages={913--921}, + year={1979}, + publisher={IEEE} + } +""" +function _solvehamiltonianare(H::Any) + + S = schur(H) + S = ordschur(S, real(S.values).<0) + + (m, n) = size(S.Z) + U11 = S.Z[1:div(m, 2), 1:div(n,2)] + U21 = S.Z[div(m,2)+1:m, 1:div(n,2)] + + return U21/U11 +end + +"""`[solution] = _solvematrixequations(P::ExtendedStateSpace, gamma::Number)` + +Solves the dual matrix equations in the γ-iterations (equations 7-12 in Doyle). +""" +function _solvematrixequations(P::ExtendedStateSpace, gamma::Number) + + A = P.A + B1 = P.B1 + B2 = P.B2 + C1 = P.C1 + C2 = P.C2 + D11 = P.D11 + D12 = P.D12 + D21 = P.D21 + D22 = P.D22 + + P1 = size(C1,1); + P2 = size(C2,1); + M1 = size(B1,2); + M2 = size(B2,2); + + gammaSq = gamma*gamma; + + B = [B1 B2] + C = [C1; C2] + + # Equation (7) + D1dot = [D11 D12]; + R = [-gammaSq*I zeros(M1,M2); zeros(M2,M1) zeros(M2,M2)] + D1dot'*D1dot; + + # Equation (8) + Ddot1 = [D11; D21]; + Rbar = [-gammaSq*I zeros(P1,P2); zeros(P2,P1) zeros(P2,P2)] + Ddot1*Ddot1'; + + # Form hamiltonian for X and Y, equation (9) and (10) + HX = [A zeros(size(A)); -C1'*C1 -A'] - ([B; -C1'*D1dot]/R)*[D1dot'*C1 B']; + HY = [A' zeros(size(A)); -B1*B1' -A] - ([C';-B1*Ddot1']/Rbar)*[Ddot1*B1' C]; + + # Solve matrix equations + Xinf = _solvehamiltonianare(HX) + Yinf = _solvehamiltonianare(HY) + + # Equation (11) + F = - R \ (D1dot'*C1+B'*Xinf) + + # Equation (12) + H = - (B1 * Ddot1' + Yinf * C') / Rbar + + return Xinf, Yinf, F, H +end + +"""`[flag]=_gammaiterations(A, B1, B2, C1, C2, D11, D12, D21, D22, maxIter, interval, verbose, tolerance)` + +Rune the complete set of γ-iterations over a specified search interval with a +set number of iterations. It is possible to break the algorithm if the number +of iterations become too large. This should perhaps be tken care of by +specifying an interval and tolerance for γ. In addition, the algorithm simply +terminates without a solution if the maximum possible gamma on the defined +interval is infeasible. Here we could consider increasing the bounds somewhat +and warn the user if this occurrs. +""" +function _gammaiterations(P::ExtendedStateSpace, maxIter::Number, interval::Tuple, verbose::Bool, tolerance::Number) + + XinfFeasible, YinfFeasible, FinfFeasible, HinfFeasible, gammFeasible = [],[],[],[],[] + + gamma = maximum(interval) + + for iteration = 1:maxIter + + # Solve the matrix equations + Xinf, Yinf, F, H = _solvematrixequations(P, gamma) + + # Check Feasibility + isFeasible = _checkfeasibility(Xinf, Yinf, gamma, tolerance, iteration; verbose=verbose) + + if isFeasible + XinfFeasible = Xinf + YinfFeasible = Yinf + FinfFeasible = F + HinfFeasible = H + gammFeasible = gamma + gamma = gamma - abs.(interval[2]-interval[1]) / (2^iteration) + else + gamma = gamma + abs.(interval[2]-interval[1]) / (2^iteration) + if gamma > maximum(interval) + break + end + end + end + return XinfFeasible, YinfFeasible, FinfFeasible, HinfFeasible, gammFeasible +end + +"""`[Pbar, Ltrans12, Rtrans12, Ltrans21, Rtrans21] = _transformP2Pbar(P::ExtendedStateSpace)` + +Transform the original system P to a new system Pbar, in which D12bar = [0; I] +and D21bar = [0 I] in order to satisfy the feasibility assumption A3 (see Doyle) +""" +function _transformp2pbar(P::ExtendedStateSpace) + + # Compute the transformation + Ltrans12, Rtrans12 = _scalematrix(P.D12) + Ltrans21, Rtrans21 = _scalematrix(P.D21) + + # Transform the system + Abar = P.A + B1bar = P.B1*Rtrans21 + B2bar = P.B2*Rtrans12 + C1bar = Ltrans12*P.C1 + C2bar = Ltrans21*P.C2 + D11bar = Ltrans12*P.D11*Rtrans21 + D12bar = Ltrans12*P.D12*Rtrans12 + D21bar = Ltrans21*P.D21*Rtrans21 + D22bar = Ltrans21*P.D22*Rtrans12 + Pbar = ss(Abar, B1bar, B2bar, C1bar, C2bar, D11bar, D12bar, D21bar, D22bar) + + return Pbar, Ltrans12, Rtrans12, Ltrans21, Rtrans21 +end + +"""`[Tl, Tr] = _scalematrix(A::AbstractMatrix; method::String)` + +Find a left and right transform of A such that Tl*A*Tr = [I, 0], or +Tl*A*Tr = [I; 0], depending on the dimensionality of A. +""" +function _scalematrix(A::AbstractMatrix; method="QR"::String) + # Check the rank condition + if (minimum(size(A)) > 0) + if rank(A) != minimum(size(A)) + error(ErrorException("Cannot scale the system, assumption A2 is violated")) + end + else + error(ErrorException("Cannot scale the system, minimum size of A must begreater than 0")) + end + + # Perform scaling with the cosen method + if method == "QR" + return _coordinatetransformqr(A) + elseif method == "SVD" + return _coordinatetransformsvd(A) + else + error(ErrorException(string("The method", method," is not supported, use 'QR' or 'SVD' instad."))) + end +end + +"""`[Tl, Tr] = _computeCoordinateTransformQR(A::AbstractMatrix)` + +Use the QR decomposition to find a transformaiton [Tl, Tr] such that +Tl*A*Tr becomes [0;I], [0 I] or I depending on the dimensionality of A. +""" +function _coordinatetransformqr(A::AbstractMatrix) + m, n = size(A) + if m == n + # Square matrix with full rank + Q, R = qr(A) + LeftTransform = Q' + RightTransform = inv(R) + elseif m > n + # Rectangular matrix with rank = n + Q, R = qr(A) + LeftTransform = [Q[:,(n+1):(m)]'; Q[:,1:n]'] + RightTransform = inv(R[1:n,1:n]) + elseif m < n + # Rectangular matrix with rank = m + Q, R = qr(A') + LeftTransform = inv(R[1:m,1:m])' + RightTransform = [Q[:,m+1:n] Q[:,1:m]] + end + return LeftTransform, RightTransform +end + +"""`[Tl, Tr] = _computeCoordinateTransformSVD(A::AbstractMatrix)` + +Use the SVD to find a transformaiton [Tl, Tr] such that +Tl*A*Tr becomes [0;I], [0 I] or I depending on the dimensionality of A. +""" +function _coordinatetransformsvd(A::AbstractMatrix) + m, n = size(A) + if m == n + # Square matrix with full rank + U, S, V = svd(A) + LeftTransform = inv(Diagonal(S))*U' + RightTransform = V + elseif m > n + # Rectangular matrix with rank = n + U, S, V = svd(A; full=true) + LeftTransform = [U[:,n+1:m]';inv(Diagonal(S))*U[:,1:n]'] + RightTransform = V + elseif m < n + # Rectangular matrix with rank = m + U, S, V = svd(A; full=true) + LeftTransform = inv(Diagonal(S))*U' + RightTransform = [V[:,m+1:n] V[:,1:m]] + end + return LeftTransform, RightTransform +end + +"""`[P] = hInf_partition(G, WS, WU, WT)` + +This is a relly ugly function which should be re-written using the new type +ExtendedStateSpace in the event of time. The reason for it's current appearance +is that I wanted to be absolutely sure that it was doing what I wanted it to do. + +Transform a SISO or MIMO system G, with weighting functions WS, WU, WT into +and LFT with an isolated controller, and write the resulting system, P(s), +on a state-space form. Valid inputs for G are transfer functions (with dynamics, +can be both MIMO and SISO, both in tf and ss forms). Valid inputs for the +weighting functions are empty entries, numbers (static gains), and transfer +fucntion objects on a the trasfer function or the state-space form. +""" +function hinfpartition(G::Any, WS::Any, WU::Any, WT::Any) + # Convert the systems into state-space form + Ag, Bg, Cg, Dg = _input2ss(G) + Asw, Bsw, Csw, Dsw = _input2ss(WS) + Auw, Buw, Cuw, Duw = _input2ss(WU) + Atw, Btw, Ctw, Dtw = _input2ss(WT) + + # Check that the system is realizable + if size(Cg, 1) != size(Btw,2) && size(Btw,2) != 0 + println([size(Cg, 1) , size(Btw,2)]) + error(DimensionMismatch("You must have the same number of outputs y=C2xg+D21w+D22u as there are inputs to WT")) + end + if size(Cg, 1) != size(Bsw,2) && size(Bsw,2) != 0 + println([size(Cg, 1) , size(Bsw,2)]) + error(DimensionMismatch("You must have the same number of states x=Agxg+B1w+B2u as there are inputs to WS")) + end + if size(Bg, 2) != size(Buw,2) && size(Buw,2) != 0 + println([size(Bg, 2) , size(Buw,2)]) + error(DimensionMismatch("You must have the same number of controls u as there are inputs to WU")) + end + if (size(Ag,1)==0 || size(Ag,2)==0 || size(Bg,1)==0 || size(Bg,2)==0 || + size(Cg,1)==0 || size(Cg,2)==0 || size(Dg,1)==0 || size(Dg,2)==0) + error(DimensionMismatch("Expansion of systems dimensionless A,B,C or D is not yet supported")) + end + + # Form A + (mAg, nAg) = size(Ag) + (mAsw,nAsw) = size(Asw) + (mAuw,nAuw) = size(Auw) + (mAtw,nAtw) = size(Atw) + + if size(Bsw,1) != 0 && size(Bsw,2) != 0 + BswCg = Bsw*Cg + else + BswCg = zeros(mAsw,nAg) + end + if size(Btw,1) != 0 && size(Btw,2) != 0 + BtwCg = Btw*Cg + else; + BtwCg = zeros(mAtw,nAg) + end + + println([(mAg, nAg), (mAsw,nAsw), (mAuw,nAuw), (mAtw,nAtw)]) + A =[Ag zeros(mAg, nAsw) zeros(mAg,nAuw) zeros(mAg,nAtw) ; + -BswCg Asw zeros(mAsw,nAuw) zeros(mAsw,nAtw); + zeros(mAuw,nAg) zeros(mAuw,nAsw) Auw zeros(mAuw,nAtw); + BtwCg zeros(mAtw,nAsw) zeros(mAtw,nAuw) Atw ] + + if size(Buw,2) == 0; Buw = zeros(0,size(Bg,2)); end; + + (mBg, nBg) = size(Bg) + (mBsw,nBsw) = size(Bsw) + (mBuw,nBuw) = size(Buw) + (mBtw,nBtw) = size(Btw) + + Bw=[zeros(mBg,nBsw); Bsw ; zeros(mBuw,nBsw); zeros(mAtw,nBsw)]; + Bu=[Bg ; zeros(mBsw,nBg); Buw; zeros(mAtw,nBg)]; + + (mCg,nCg) = size(Cg) + (mCsw,nCsw) = size(Csw) + (mCuw,nCuw) = size(Cuw) + (mCtw,nCtw) = size(Ctw) + + if size(Dsw,1) != 0 && size(Dsw,1) != 0; DswCg = Dsw*Cg; else; DswCg = zeros(0,nAg); end; + if size(Dtw,1) != 0 && size(Dtw,1) != 0; DtwCg = Dtw*Cg; else; DtwCg = zeros(0,nAg); end; + + Cz=[-DswCg Csw zeros(mCsw,nAuw) zeros(mCsw,nAtw); + zeros(mCuw,nAg) zeros(mCuw,nAsw) Cuw zeros(mCuw,nAtw); + DtwCg zeros(mCtw,nAsw) zeros(mCtw,nAuw) Ctw ]; + Cy=[-Cg zeros(mCg,nAsw) zeros(mCg,nAuw) zeros(mCg,nAtw)]; + + + if size(Duw,2) == 0; Duw = zeros(0,size(Bg,2)); end; + + (mDg, nDg) = size(Dg) + (mDsw,nDsw) = size(Dsw) + (mDuw,nDuw) = size(Duw) + (mDtw,nDtw) = size(Dtw) + + Dzw=[Dsw; zeros(mDuw,nDsw); zeros(mDtw,nDsw)]; + Dzu=[zeros(mDsw,nDuw); Duw; zeros(mDtw,nDuw)]; + Dyw=Matrix{Float64}(I, mCg, nDuw) + Dyu=-Dg; + + P = ss(A, Bw, Bu, Cz, Cy, Dzw, Dzu, Dyw, Dyu) + return P +end + +"""`convert_input_to_ss(H)` + +Help function used for type conversion in hInf_partition() +""" +function _input2ss(H::Any) + if isa(H, LTISystem) + if isa(H, TransferFunction) + Hss = ss(H) + else + Hss = H + end + Ah, Bh, Ch, Dh = Hss.A, Hss.B, Hss.C, Hss.D + elseif isa(H, Number) + Ah, Bh, Ch, Dh = zeros(0,0), zeros(0,1), zeros(1,0), H*ones(1,1) + else + Ah, Bh, Ch, Dh = zeros(0,0), zeros(0,0), zeros(0,0), zeros(0,0) + end + return Ah, Bh, Ch, Dh +end + +"""`hInf_signals(P::ExtendedStateSpace, G::LTISystem, C::LTISystem)` + +Use the extended state-space model, a plant and the found controller to extract +the closed loop transfer functions operating solely on the state-space. + + Pcl : w → z : From input to the weighted functions + S : w → e : From input to error + CS : w → u : From input to control + T : w → y : From input to output +""" +function hinfsignals(P::ExtendedStateSpace, G::LTISystem, C::LTISystem) + + A, B1, B2, C1, C2, D11, D12, D21, D22 = ssdata(P) + Ag, Bg, Cg, Dg = ssdata(ss(G)) + Ac, Bc, Cc, Dc = ssdata(ss(C)) + + # Precompute the inverse + M = inv(I - Dc * D22) + A11 = A + B2 * M * Dc * C2 + A12 = B2 * M * Cc + A21 = Bc * C2 + Bc * D22 * M * Dc * C2 + A22 = Ac + Bc * D22 * M * Cc + B11 = B1 + B2 * M * Dc * D21 + B21 = Bc * D21 + Bc * D22 * M * Dc * D21 + + C_w2z_11 = C1 + D12 * M * Dc * C2 + C_w2z_12 = D12 * M * Cc + D_w2z = D11 + D12 * M * Dc * D21 + + Pw2z = ss([A11 A12; A21 A22], [B11; B21], [C_w2z_11 C_w2z_12], D_w2z) + + C_w2e_11 = C2 + D22 * M * Dc * C2 + C_w2e_12 = D22 * M * Cc + D_w2e = D21 + D22 * M * Dc * D21 + + Pw2e = ss([A11 A12; A21 A22], [B11; B21], [C_w2e_11 C_w2e_12], D_w2e) + + C_w2u_11 = M * Dc * C2 + C_w2u_12 = M * Cc + D_w2u = M * Dc * D21 + + Pw2u = ss([A11 A12; A21 A22], [B11; B21], [C_w2u_11 C_w2u_12], D_w2u) + + Abar = [A11 A12; A21 A22] + Bbar = [B11; B21] + C_w2y_11 = M * Dc * C2 + C_w2y_12 = M * Cc + D_w2y = M * Dc * D21 + Cbar1 = [Cg zeros(size(Cg,1), size(Abar,2)-size(Cg,2))] + Cbar2 = [Dg*C_w2y_11 Dg*C_w2y_12] + Dbar = Dg * M * Dc * D21 + + Pw2y = ss(Abar, Bbar, Cbar1 + Cbar2, Dbar) + + Pcl = Pw2z + S = Pw2e + CS = Pw2u + T = Pw2y + + return Pcl, S, CS, T +end + + +"""`bilineard2c(Ad::AbstractArray, Bd::AbstractArray, Cd::AbstractArray, Dd::AbstractArray, Ts::Number; tolerance=1e-12)` + +Balanced Bilinear transformation in State-Space. This method computes a +continuous time equivalent of a discrete time system, such that + + G_c(z) = z2s[G_d(z)] + +in a manner which accomplishes the following + (i) Preserves the infinity L-infinity norm over the transformation + (ii) Finds a system which balances B and C, in the sense that ||B||_2=||C||_2 + (iii) Satisfies G_d(z) = s2z[z2s[G_d(z)]] for some map s2z[] +""" +function bilineard2c(Ad::AbstractArray, Bd::AbstractArray, Cd::AbstractArray, Dd::AbstractArray, Ts::Number; tolerance=1e-12) + + Id = Matrix{Float64}(I, size(Ad,1), size(Ad,2)) + + Pd = Ad - Id + Qd = Ad + Id + ialpha = 2/Ts #Should be this, but the nyquist frequency doesnt add up unless + ialpha = 1/Ts + + Ac = ialpha*(Pd/Qd) + Bc = Qd\Bd + Cc = 2*ialpha*(Cd/Qd) + Dc = Dd - (Cd/Qd)*Bd + + # Scaling for improved numerical stability + σB = maximum(svd(Bc).S) + σC = maximum(svd(Cc).S) + if σB > tolerance && σC > tolerance + λd = sqrt(σB / σC) + else + λd = 1 + error("Warning, the problem is poorly cnditioned. Consider an alternate discretization scheme.") + end + Bc /= λd + Cc *= λd + + return Ac, Bc, Cc, Dc +end + +"""`bilineard2c(sys::StateSpace)` + +Applies a Balanced Bilinear transformation to continuous-time statespace object +""" +function bilineard2c(sys::StateSpace) + Ad, Bd, Cd, Dd = ssdata(sys) + Ts = sys.Ts + + if Ts <= 0; error("Error, the input must be a discrete time system."); end + + Ac, Bc, Cc, Dc = bilineard2c(Ad, Bd, Cd, Dd, Ts) + return ss(Ac, Bc, Cc, Dc) +end + +"""`bilineard2c(sys::ExtendedStateSpace)` + +Applies a Balanced Bilinear transformation to continuous-time extended statespace object +""" +function bilineard2c(sys::ExtendedStateSpace) + Ad = get_A(sys) + Bd = get_B(sys) + Cd = get_C(sys) + Dd = get_D(sys) + Ts = sys.Ts + + m1 = size(get_B1(sys),2) + m2 = size(get_B2(sys),2) + p1 = size(get_C1(sys),1) + p2 = size(get_C2(sys),1) + + if Ts <= 0; error("Error, the input must be a discrete time system."); end + + Ac, Bc, Cc, Dc = bilineard2c(Ad, Bd, Cd, Dd, Ts) + + A = Ac + B1 = Bc[:,1:m1] + B2 = Bc[:,(m1+1):(m1+m2)] + C1 = Cc[1:p1,:] + C2 = Cc[(p1+1):(p1+p2),:] + D11 = Dc[1:p1,1:m1] + D12 = Dc[1:p1,(m1+1):(m1+m2)] + D21 = Dc[(p1+1):(p1+p2),1:m1] + D22 = Dc[(p1+1):(p1+p2),(m1+1):(m1+m2)] + + return ss(A, B1, B2, C1, C2, D11, D12, D21, D22) +end + +"""`bilinearc2d(Ac::AbstractArray, Bc::AbstractArray, Cc::AbstractArray, Dc::AbstractArray, Ts::Number; tolerance=1e-12)` + +Balanced Bilinear transformation in State-Space. This method computes a +discrete time equivalent of a continuous-time system, such that + + G_d(z) = s2z[G_c(s)] + +in a manner which accomplishes the following + (i) Preserves the infinity L-infinity norm over the transformation + (ii) Finds a system which balances B and C, in the sense that ||B||_2=||C||_2 + (iii) Satisfies G_c(s) = z2s[s2z[G_c(s)]] for some map z2s[] +""" +function bilinearc2d(Ac::AbstractArray, Bc::AbstractArray, Cc::AbstractArray, Dc::AbstractArray, Ts::Number; tolerance = 1e-12) + + Id = Matrix{Float64}(I, size(Ac,1), size(Ac,2)) + alpha = Ts/2 #Should be this, but the nyquist frequency doesnt add up + alpha = Ts + + # Check that the bilinear tranformation is possible + if minimum(svd(Id - alpha * Ac).S) < 1e-12 + error("The transformation is extremely poorly conditioned, with min(svd(Id - alpha * Ac).S) < 1e-12. Consider an alternate discretization scheme.") + end + + PP = Id - alpha * Ac + QQ = Id + alpha * Ac + + Ad = PP \ QQ + Bd = (PP\Bc) + Cd = 2 * alpha * (Cc/PP) + + # Scaling for improved numerical stability + σB = maximum(svd(Bd).S) + σC = maximum(svd(Cd).S) + if σB > tolerance && σC > tolerance + λc = sqrt(σB / σC) + else + λc = 1 + error("Warning, the problem is poorly cnditioned. Consider an alternate discretization scheme.") + end + + Bd /= λc + Cd *= λc + + Dd = alpha * Cc/PP*Bc + Dc + return Ad, Bd, Cd, Dd, Ts +end + +"""`bilinearc2d(sys::StateSpace, Ts::Number)` + +Applies a Balanced Bilinear transformation to a discrete-time statespace object +""" +function bilinearc2d(sys::StateSpace, Ts::Number) + Ac, Bc, Cc, Dc = ssdata(sys) + + if sys.Ts > 0 + error("Error, the input to bilinear_z2s() must be a continuous time system.") + end + if Ts <= 0 + error("Error, the the discretization time Ts must be positive.") + end + + Ad, Bd, Cd, Dd = bilinearc2d(Ac, Bc, Cc, Dc, Ts) + return ss(Ad, Bd, Cd, Dd, Ts) +end + +"""`bilinearc2d(sys::ExtendedStateSpace, Ts::Number)` + +Applies a Balanced Bilinear transformation to a discrete-time extended statespace object +""" +function bilinearc2d(sys::ExtendedStateSpace, Ts::Number) + Ac = get_A(sys) + Bc = get_B(sys) + Cc = get_C(sys) + Dc = get_D(sys) + + m1 = size(get_B1(sys),2) + m2 = size(get_B2(sys),2) + p1 = size(get_C1(sys),1) + p2 = size(get_C2(sys),1) + + if sys.Ts > 0 + error("Error, the input to hInf_bilinear_s2z() must be a continuous time system.") + end + if Ts <= 0 + error("Error, the the discretization time Ts must be positive.") + end + + Ad, Bd, Cd, Dd = bilinearc2d(Ac, Bc, Cc, Dc, Ts) + + A = Ad + B1 = Bd[:,1:m1] + B2 = Bd[:,(m1+1):(m1+m2)] + C1 = Cd[1:p1,:] + C2 = Cd[(p1+1):(p1+p2),:] + D11 = Dd[1:p1,1:m1] + D12 = Dd[1:p1,(m1+1):(m1+m2)] + D21 = Dd[(p1+1):(p1+p2),1:m1] + D22 = Dd[(p1+1):(p1+p2),(m1+1):(m1+m2)] + + return ss(A, B1, B2, C1, C2, D11, D12, D21, D22, Ts) +end diff --git a/src/matrix_comps.jl b/src/matrix_comps.jl index d38707240..32d5a6293 100644 --- a/src/matrix_comps.jl +++ b/src/matrix_comps.jl @@ -12,9 +12,9 @@ function care(A, B, Q, R) B*inv(R)*B' catch y if y isa SingularException - error("R must be non-singular in care.") + error("R must be non-singular.") else - throw(y) + throw(t) end end diff --git a/src/plotting.jl b/src/plotting.jl index 62beec140..3a1e9430a 100644 --- a/src/plotting.jl +++ b/src/plotting.jl @@ -1,5 +1,5 @@ import Colors -export lsimplot, stepplot, impulseplot, bodeplot, nyquistplot, sigmaplot, marginplot, setPlotScale, gangoffour, gangoffourplot, gangofseven, pzmap, pzmap!, nicholsplot +export lsimplot, stepplot, impulseplot, bodeplot, nyquistplot, sigmaplot, marginplot, setPlotScale, gangoffour, gangoffourplot, gangofseven, pzmap, pzmap!, nicholsplot, specificationplot getColorSys(i,Nsys) = convert(Colors.RGB,Colors.HSV(360*((i-1)/Nsys)^1.5,0.9,0.8)) function getStyleSys(i,Nsys) @@ -38,25 +38,11 @@ function setPlotScale(str::AbstractString) _PlotScale, _PlotScaleFunc, _PlotScaleStr = plotSettings end -""" -Get atributes from xlims or ylims -default to extrema(wmag) if xlims/ylims not defined or empty -""" -function getlims(xylims, plotattributes, wmag) - lims = get(plotattributes, xylims, extrema(wmag)) - if lims isa Tuple{<:Number, <:Number} # If x/ylims not supplied as empty - return lims - else - return extrema(wmag) - end -end - -function getLogTicks(x, minmax) - minx, maxx = minmax +function getLogTicks(x) major_minor_limit = 6 minor_text_limit = 8 - min = ceil(log10(minx)) - max = floor(log10(maxx)) + min = ceil(log10(minimum(x))) + max = floor(log10(maximum(x))) major = 10 .^ collect(min:max) if Plots.backend() != Plots.GRBackend() majorText = [latexstring("\$10^{$(round(Int64,i))}\$") for i = min:max] @@ -71,7 +57,7 @@ function getLogTicks(x, minmax) minorText = ["$j*10^{$(round(Int64,i))}" for i = (min-1):(max+1) for j = 2:9] end - ind = findall(minx .<= minor .<= maxx) + ind = findall(minimum(x) .<= minor .<= maximum(x)) minor = minor[ind] minorText = minorText[ind] if length(minor) > minor_text_limit @@ -208,27 +194,6 @@ for (func, title, typ) = ((step, "Step Response", Stepplot), (impulse, "Impulse end -""" - processfreqplot(plottype::Symbol, system::LTISystem, [w]) - processfreqplot(plottype::Symbol, system::AbstractVector{<:LTISystem}, [w]) - - Calculate default frequency vector and put system in array of not already array. - Check that system dimensions are compatible. -""" -processfreqplot(plottype::Symbol, system::LTISystem, args...) = - processfreqplot(plottype, [system], args...) -# Catch when system is not vector, with and without frequency input - -# Cantch correct form -function processfreqplot(plottype::Symbol, systems::AbstractVector{<:LTISystem}, - w = _default_freq_vector(systems, plottype)) - - if !_same_io_dims(systems...) - error("All systems must have the same input/output dimensions") - end - return systems, w -end - @userplot Bodeplot ## FREQUENCY PLOTS ## @@ -240,13 +205,24 @@ optionally provided. `kwargs` is sent as argument to Plots.plot.""" bodeplot -@recipe function bodeplot(p::Bodeplot; plotphase=true, ylimsphase=()) - systems, w = processfreqplot(:bode, p.args...) +@recipe function bodeplot(p::Bodeplot; plotphase=true) + systems = p.args[1] + if !isa(systems, AbstractArray) + systems = typeof(systems)[systems] + end + if length(p.args) >= 2 + w = p.args[2] + else + w = _default_freq_vector(systems, :bode) + end + if !_same_io_dims(systems...) + error("All systems must have the same input/output dimensions") + end ny, nu = size(systems[1]) s2i(i,j) = LinearIndices((nu,(plotphase ? 2 : 1)*ny))[j,i] layout --> ((plotphase ? 2 : 1)*ny,nu) nw = length(w) - xticks --> getLogTicks(w, getlims(:xlims, plotattributes, w)) + xticks --> getLogTicks(w) for (si,s) = enumerate(systems) mag, phase = bode(s, w)[1:2] @@ -270,7 +246,7 @@ bodeplot yscale --> _PlotScaleFunc xscale --> :log10 if _PlotScale != "dB" - yticks --> getLogTicks(magdata, getlims(:ylims, plotattributes, magdata)) + yticks --> getLogTicks(magdata) end xguide --> xlab yguide --> "Magnitude $_PlotScaleStr" @@ -284,11 +260,9 @@ bodeplot if !plotphase continue end - @series begin grid --> true xscale --> :log10 - ylims := ylimsphase yguide --> "Phase (deg)" subplot --> s2i(2i,j) xguide --> "Frequency (rad/s)" @@ -313,8 +287,8 @@ end yscale --> :log10 xscale --> :log10 yguide --> "Magnitude" - xticks --> getLogTicks(w, getlims(:xlims, plotattributes, w)) - yticks --> getLogTicks(magdata, getlims(:ylims, plotattributes, magdata)) + xticks --> getLogTicks(w) + yticks --> getLogTicks(magdata) x := w; y := magdata () end @@ -332,7 +306,7 @@ end xscale --> :log10 yguide --> "Phase (deg)" xguide --> "Frequency (rad/s)" - xticks --> getLogTicks(w, getlims(:xlims, plotattributes, w)) + xticks --> getLogTicks(w) x := w; y := phasedata () end @@ -355,8 +329,15 @@ the sensitivity and complementary sensitivity functions. `kwargs` is sent as argument to plot.""" nyquistplot @recipe function nyquistplot(p::Nyquistplot; gaincircles=true) - systems, w = processfreqplot(:nyquist, p.args...) + systems = p.args[1] + if !isa(systems,AbstractArray) + systems = [systems] + end + if !_same_io_dims(systems...) + error("All systems must have the same input/output dimensions") + end ny, nu = size(systems[1]) + w = length(p.args) < 2 ? _default_freq_vector(systems, :nyquist) : p.args[2] nw = length(w) layout --> (ny,nu) s2i(i,j) = LinearIndices((ny,nu))[j,i] @@ -444,7 +425,11 @@ nicholsplot val = 0.85, fontsize = 10) - systems, w = processfreqplot(:nyquist, p.args...) + systems, w = p.args[1:2] + + if !_same_io_dims(systems...) + error("All systems must have the same input/output dimensions") + end ny, nu = size(systems[1]) if !iscontinuous(systems[1]) @@ -572,6 +557,10 @@ nicholsplot end +nicholsplot(systems::Vector{T};kwargs...) where {T<:LTISystem} = +nicholsplot(systems, _default_freq_vector(systems, :nyquist);kwargs...) +nicholsplot(sys::LTISystem, args...; kwargs...) = nicholsplot([sys],args...; kwargs...) + @userplot Sigmaplot """`sigmaplot(sys, args...)`, `sigmaplot(LTISystem[sys1, sys2...], args...)` @@ -581,7 +570,10 @@ frequency vector `w` can be optionally provided. `kwargs` is sent as argument to Plots.plot.""" sigmaplot @recipe function sigmaplot(p::Sigmaplot) - systems, w = processfreqplot(:sigma, p.args...) + systems, w = p.args[1:2] + if !_same_io_dims(systems...) + error("All systems must have the same input/output dimensions") + end ny, nu = size(systems[1]) nw = length(w) title --> "Sigma Plot" @@ -604,6 +596,9 @@ sigmaplot end end end +sigmaplot(systems::Vector{T}; kwargs...) where {T<:LTISystem} = +sigmaplot(systems, _default_freq_vector(systems, :sigma); kwargs...) +sigmaplot(sys::LTISystem, args...; kwargs...) = sigmaplot([sys], args...; kwargs...) """`fig = marginplot(sys::LTISystem [,w::AbstractVector]; kwargs...)`, `marginplot(sys::Vector{LTISystem}, w::AbstractVector; kwargs...)` @@ -611,8 +606,10 @@ Plot all the amplitude and phase margins of the system(s) `sys`. A frequency vector `w` can be optionally provided. `kwargs` is sent as argument to Plots.plot.""" -function marginplot(systems::Union{AbstractVector{T},T}, args...; kwargs...) where T<:LTISystem - systems, w = processfreqplot(:bode, systems, args...) +function marginplot(systems::Vector{T}, w::AbstractVector; kwargs...) where T<:LTISystem + if !_same_io_dims(systems...) + error("All systems must have the same input/output dimensions") + end ny, nu = size(systems[1]) fig = bodeplot(systems, w, kwargs...) s2i(i,j) = LinearIndices((ny,2nu))[j,i] @@ -659,6 +656,10 @@ function marginplot(systems::Union{AbstractVector{T},T}, args...; kwargs...) whe end return fig end +marginplot(systems::Vector{T}; kwargs...) where {T<:LTISystem} = +marginplot(systems, _default_freq_vector(systems, :bode); kwargs...) +marginplot(sys::LTISystem, args...; kwargs...) = marginplot([sys], args...; kwargs...) + # HELPERS: @@ -745,3 +746,111 @@ end function gangoffourplot(P::LTISystem,C::LTISystem, args...; plotphase=false, kwargs...) gangoffourplot(P,[C], args...; plotphase=plotphase, kwargs...) end + + + + +@userplot Specificationplot +"""`specificationplot(Any[S,CS,T], Any[WS,WU,WT], args...)` + +This function visualizes the control synthesis using the hInf_synthesize with +the three weighting functions {WS(jω), WU(jω), WT(jω)} inverted and scaled by γ, +against the the corresponding transfer fucntions {S(jω), C(jω)S(jω), T(jω)}, to +verify visually that the specifications are met. This may be run using both MIMO +and SISO systems. + +`kwargs` is sent as argument to Plots.plot.""" +specificationplot +@recipe function specificationplot(p::Specificationplot; + wint = (-3,5), + wnum = 101, + s_labels = ["\$\\sigma (S(j\\omega))\$", + "\$\\sigma (C(j\\omega)S(j\\omega))\$", + "\$\\sigma (T(j\\omega))\$"], + w_labels = ["\$\\gamma \\sigma (W_S(j\\omega)^{-1})\$", + "\$\\gamma \\sigma (W_U(j\\omega)^{-1})\$", + "\$\\gamma \\sigma (W_T(j\\omega)^{-1})\$"], + colors = [:red, :blue, :green]) + + sensitivityfunctions, weightfunctions, gamma = p.args[1:3] + + title --> "Specification sigma plot" + xguide --> "Frequency (rad/s)", + yguide --> "Singular Values $_PlotScaleStr" + + w = [10^i for i in range(wint[1], stop=wint[2], length=wnum)] + + # Plot the sensitivity functions + for (index, G) in enumerate(sensitivityfunctions) + println(index) + if G isa Number || G isa LTISystem + singval = sigma(ss(G), w)[1] + if _PlotScale == "dB" + singval = 20*log10.(singval) + end + for i in 1:size(singval, 2) + @series begin + xscale --> :log10 + yscale --> _PlotScaleFunc + linestyle --> :dash + linecolor --> colors[mod(index-1,3)+1] + label --> (i==1 ? s_labels[mod(index-1,3)+1] : "") + w, singval[:, i] + end + end + end + end + + ## Plot the weight functions + for (index, W) in enumerate(weightfunctions) + if W isa Number + W = ss(W) + end + if W isa LTISystem + if size(W) != (1,1) + error(ErrorException("We can currently only handle SISO weight funcitions in the visualization")) + end + singval = sigma(gamma/tf(W), w)[1] + if _PlotScale == "dB" + singval = 20*log10.(singval) + end + for i in 1:size(singval, 2) + weightlabel = (i==1 ? w_labels[mod(index-1,3)+1] : "") + @series begin + xscale --> :log10 + yscale --> _PlotScaleFunc + linestyle --> :dot + linecolor --> colors[mod(index-1,3)+1] + width --> 2 + label --> (i==1 ? w_labels[mod(index-1,3)+1] : "") + w, singval[:, i] + end + end + end + end +end + +specificationplot( + sens::Vector{T}, + weight::Vector{T}, + gamma::Number; + kwargs... +) where T<:LTISystem = specificationplot( + sens, + weight, + gamma; + kwargs... +) + +# Case where a ingle sensitivity function (for instance the closed loop TF from +# disturbance to output) and the gain γ +specificationplot( + sens::T, + gamma::Number; + kwargs... +) where T<:LTISystem = specificationplot( + [sens], + [1], + gamma; + kwargs... +) diff --git a/src/types/ExtendedStateSpace.jl b/src/types/ExtendedStateSpace.jl new file mode 100644 index 000000000..d2a9fd519 --- /dev/null +++ b/src/types/ExtendedStateSpace.jl @@ -0,0 +1,190 @@ +##################################################################### +## Data Type Declarations ## +##################################################################### + +struct ExtendedStateSpace{T, MT<:AbstractMatrix{T}} <: LTISystem + A::MT + B1::MT + B2::MT + C1::MT + C2::MT + D11::MT + D12::MT + D21::MT + D22::MT + Ts::Float64 + nx::Int + nw::Int + nu::Int + nz::Int + ny::Int + function ExtendedStateSpace{T, MT}(A::MT, B1::MT, B2::MT,C1::MT,C2::MT, + D11::MT, D12::MT, D21::MT, D22::MT, Ts::Float64) where {T, MT <: AbstractMatrix{T}} + nx = size(A, 1) + nw = size(B1, 2) + nu = size(B2, 2) + nz = size(C1, 1) + ny = size(C2, 1) + + if size(A, 2) != nx && nx != 0 + error("A must be square") + elseif size(B1, 1) != nx + error("B1 must have the same row size as A") + elseif size(B2, 1) != nx + error("B2 must have the same row size as A") + elseif size(C1, 2) != nx + error("C1 must have the same column size as A") + elseif size(C2, 2) != nx + error("C2 must have the same column size as A") + elseif nw != size(D11, 2) + error("D11 must have the same column size as B1") + elseif nw != size(D21, 2) + error("D12 must have the same column size as B1") + elseif nu != size(D12, 2) + error("D12 must have the same column size as B2") + elseif nu != size(D22, 2) + error("D22 must have the same column size as B2") + elseif nz != size(D11, 1) + error("D11 must have the same row size as C1") + elseif nz != size(D12, 1) + error("D12 must have the same row size as C1") + elseif ny != size(D21, 1) + error("D11 must have the same row size as C12") + elseif ny != size(D22, 1) + error("D12 must have the same row size as C2") + end + + # Validate sampling time + if Ts < 0 && Ts != -1 + error("Ts must be either a positive number, 0 + (continuous system), or -1 (unspecified)") + end + new{T, MT}(A, B1, B2, C1, C2, D11, D12, D21, D22, Ts, nx, nw, nu, nz, ny) + end +end + +function ExtendedStateSpace{T,MT}(A::AbstractArray, B1::AbstractArray, B2::AbstractArray, C1::AbstractArray, C2::AbstractArray, D11::AbstractArray, D12::AbstractArray, D21::AbstractArray, D22::AbstractArray, Ts::Real) where {T, MT <: AbstractMatrix{T}} + return ExtendedStateSpace{T,Matrix{T}}(MT(to_matrix(T, A)), MT(to_matrix(T, B1)), + MT(to_matrix(T, B2)), MT(to_matrix(T, C1)), MT(to_matrix(T, C2)), + MT(to_matrix(T, D11)), MT(to_matrix(T, D12)), MT(to_matrix(T, D21)), + MT(to_matrix(T, D22)), Float64(Ts)) +end + +function ExtendedStateSpace(A::AbstractArray, B1::AbstractArray, B2::AbstractArray, C1::AbstractArray, C2::AbstractArray, D11::AbstractArray, D12::AbstractArray, D21::AbstractArray, D22::AbstractArray, Ts::Real) + # TODO: change back in 0.7 T = promote_type(eltype(A),eltype(B),eltype(C),eltype(D)) + TBC = promote_type(promote_type(eltype(B1),eltype(B2)),promote_type(eltype(C1),eltype(C2))) + TD = promote_type(promote_type(eltype(D11),eltype(D12)),promote_type(eltype(D21),eltype(D22))) + T = promote_type(promote_type(TBC,TD),eltype(A)) + @assert (typeof(to_matrix(T, A)) == typeof(to_matrix(T, B1)) == typeof(to_matrix(T, B2)) == typeof(to_matrix(T, C1)) == typeof(to_matrix(T, C2)) == typeof(to_matrix(T, D11)) == typeof(to_matrix(T, D12)) == typeof(to_matrix(T, D21))) + return ExtendedStateSpace{T,Matrix{T}}(to_matrix(T, A), to_matrix(T, B1), + to_matrix(T, B2), to_matrix(T, C1), to_matrix(T, C2), to_matrix(T, D11), + to_matrix(T, D12), to_matrix(T, D21), to_matrix(T, D22), Float64(Ts)) +end + +# Getter functions +get_A(sys::ExtendedStateSpace) = sys.A +get_B1(sys::ExtendedStateSpace) = sys.B1 +get_B2(sys::ExtendedStateSpace) = sys.B2 +get_B(sys::ExtendedStateSpace) = [sys.B1 sys.B2] +get_C1(sys::ExtendedStateSpace) = sys.C1 +get_C2(sys::ExtendedStateSpace) = sys.C2 +get_C(sys::ExtendedStateSpace) = [sys.C1; sys.C2] +get_D11(sys::ExtendedStateSpace) = sys.D11 +get_D12(sys::ExtendedStateSpace) = sys.D12 +get_D21(sys::ExtendedStateSpace) = sys.D21 +get_D22(sys::ExtendedStateSpace) = sys.D22 +get_D(sys::ExtendedStateSpace) = [sys.D11 sys.D12 ; sys.D21 sys.D22] + +get_Ts(sys::ExtendedStateSpace) = sys.Ts + +ssdata(sys::ExtendedStateSpace) = get_A(sys), get_B1(sys), get_B2(sys), get_C1(sys), get_C2(sys), get_D11(sys), get_D12(sys), get_D21(sys), get_D22(sys) + +# Funtions for number of intputs, outputs and states +ninputs(sys::ExtendedStateSpace) = size(get_D(sys), 2) +noutputs(sys::ExtendedStateSpace) = size(get_D(sys), 1) +nstates(sys::ExtendedStateSpace) = size(get_A(sys), 1) + +##################################################################### +## Math Operators ## +##################################################################### + +## EQUALITY ## +function ==(sys1::ExtendedStateSpace, sys2::ExtendedStateSpace) + return all(getfield(sys1, f) == getfield(sys2, f) for f in fieldnames(ExtendedStateSpace)) +end + +## Approximate ## +function isapprox(sys1::ExtendedStateSpace, sys2::ExtendedStateSpace) + return all(getfield(sys1, f) ≈ getfield(sys2, f) for f in fieldnames(ExtendedStateSpace)) +end + +## ADDITION ## +# not sure how to best handle this yet + +## SUBTRACTION ## +# not sure how to best handle this yet + +## NEGATION ## +# not sure how to best handle this yet + +## MULTIPLICATION ## +# not sure how to best handle this yet + +## DIVISION ## +# not sure how to best handle this yet + +##################################################################### +## Indexing Functions ## +##################################################################### +Base.ndims(::ExtendedStateSpace) = 2 # NOTE: Also for SISO systems? +Base.size(sys::ExtendedStateSpace) = (noutputs(sys), ninputs(sys)) # NOTE: or just size(get_D(sys)) +Base.size(sys::ExtendedStateSpace, d) = d <= 2 ? size(sys)[d] : 1 +Base.eltype(::Type{S}) where {S<:ExtendedStateSpace} = S + +function Base.getindex(sys::ExtendedStateSpace, inds...) + if size(inds, 1) != 2 + error("Must specify 2 indices to index statespace model") + end + rows, cols = index2range(inds...) # FIXME: ControlSystems.index2range(inds...) + return ExtendedStateSpace(copy(sys.A), sys.B[:, cols], sys.C[rows, :], sys.D[rows, cols], sys.Ts) +end + +##################################################################### +## Display Functions ## +##################################################################### + +Base.print(io::IO, sys::ExtendedStateSpace) = show(io, sys) + +function Base.show(io::IO, sys::ExtendedStateSpace) + # Compose the name vectors + println(io, typeof(sys)) + if nstates(sys) > 0 + println(io, "A = \n", _string_mat_with_headers(sys.A)) + println(io, "B1 = \n", _string_mat_with_headers(sys.B1)) + println(io, "B2 = \n", _string_mat_with_headers(sys.B2)) + println(io, "C1 = \n", _string_mat_with_headers(sys.C1)) + println(io, "C2 = \n", _string_mat_with_headers(sys.C2)) + println(io, "D11 = \n", _string_mat_with_headers(sys.D11)) + println(io, "D12 = \n", _string_mat_with_headers(sys.D12)) + println(io, "D21 = \n", _string_mat_with_headers(sys.D21)) + println(io, "D22 = \n", _string_mat_with_headers(sys.D22)) + else + println(io, "The extended statespece model has no states..!") + end + + # Print sample time + if sys.Ts > 0 + println(io, "Sample Time: ", sys.Ts, " (seconds)") + elseif sys.Ts == -1 + println(io, "Sample Time: unspecified") + end + + # Print model type + if nstates(sys) == 0 + print(io, "Static gain") + elseif iscontinuous(sys) + print(io, "Continuous-time extended state-space model") + else + print(io, "Discrete-time extended state-space model") + end +end diff --git a/src/types/ss.jl b/src/types/ss.jl index 3d5aa5b5e..518b8e78b 100644 --- a/src/types/ss.jl +++ b/src/types/ss.jl @@ -32,6 +32,10 @@ function ss(A::Union{Number,AbstractArray}, B::Union{Number,AbstractArray}, C::U ss(A, B, C, D, Ts) end +function ss(A::AbstractArray, B1::AbstractArray, B2::AbstractArray, C1::AbstractArray, C2::AbstractArray, D11::AbstractArray, D12::AbstractArray, D21::AbstractArray, D22::AbstractArray, Ts::Real=0) + return ExtendedStateSpace(A, B1, B2, C1, C2, D11, D12, D21, D22, Ts) +end + # Function for creation of static gain function ss(D::AbstractArray{T}, Ts::Real=0) where {T<:Number} ny, nu = size(D, 1), size(D, 2) diff --git a/test/runtests.jl b/test/runtests.jl index b415db570..17bb14142 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -23,7 +23,8 @@ my_tests = ["test_statespace", "test_analysis", "test_matrix_comps", "test_lqg", - "test_synthesis"] + "test_synthesis", + "test_hinf_design"] # try diff --git a/test/test_hinf_design.jl b/test/test_hinf_design.jl new file mode 100644 index 000000000..2bd58ba15 --- /dev/null +++ b/test/test_hinf_design.jl @@ -0,0 +1,731 @@ +using ControlSystems +using Test +using LinearAlgebra +using Random + +execute_tests = [true,true,true,true,true,true,true,true,true] + +@testset "H-infinity design" begin + """ + Tests for the public and private methods of the hInfSynthesis function. This + function utilizes the preexisting ControlSystems toolbox, and performs a + H-infinity synthesis using the dual Riccati equation approach. As such, + the synthesis is done in a set of steps. + + (1) Re-writing the specifications on an extended state-space form. + (2) Verifying that the resulting extended state-space object satisfies a set of + assumptions required for proceeding with the synthesis. + (3) A coordinate transform to enable the synthesis. + (4) Synthesis using the γ-iterations, checking if a solution to the H-infinity + problem exists in each iteration and applying a bisection method. + (5) Re-transforming the system to the original coordinates + (6) Verification that the computed solution is correct. + + In addition to these six ponts, the code also enables + + (7) A bilinear discretization with an inverse operation to move from continuous + to discrete time, thereby enabling approximate discrete-time synthesis. + (8) Plotting functionality to visualize the H-infinity synthesis. + (9) Three examples which can be used to demonstrate the tool. + + Many of the tests are quite intuitive, and all points (1)-(9) are tested + extensively with detailed comments for each test-set, and below is a list + of all the functions tested in this unit test + + hinfsynthesize + hinfassumptions + hinfsignals + hinfpartition + bilineard2c + bilinearc2d + _detectable + _stabilizable + _synthesizecontroller + _assertrealandpsd + _checkfeasibility + _solvehamiltonianare + _solvematrixequations + _gammaIterations + _transformp2pbar + _scalematrix + _input2ss + _coordinateTtansformqr + _coordinateTtansformsvd + + """ + + if execute_tests[1] + @testset "(1) Specifications" begin + """ + These tests make sure that the specifications are written on a suitable form + for any combination of none/empty, static gain, SISO, and MIMO + specificaitons in the weighting functions. Essentially, the tests are made + to verify that hInf_partition(G, WS, WU, WT) works as expected. + """ + + @testset "Conversion of user input" begin + + @testset "Empty and nothing" begin + + # Check that conversion of nothing is OK + A, B, C, D = ControlSystems._input2ss(nothing) + @test isa(A, Array{Float64,2}) + @test isa(B, Array{Float64,2}) + @test isa(C, Array{Float64,2}) + @test isa(D, Array{Float64,2}) + + @test size(A) == (0,0) + @test size(B) == (0,0) + @test size(C) == (0,0) + @test size(D) == (0,0) + + # Check that conversion of empty objects are OK + A, B, C, D = ControlSystems._input2ss([]) + @test isa(A, Array{Float64,2}) + @test isa(B, Array{Float64,2}) + @test isa(C, Array{Float64,2}) + @test isa(D, Array{Float64,2}) + + @test size(A) == (0,0) + @test size(B) == (0,0) + @test size(C) == (0,0) + @test size(D) == (0,0) + end + + @testset "Static gains" begin + # Fixture + number = 2.0 + + # Check that conversion of numbers are OK + A, B, C, D = ControlSystems._input2ss(number) + @test isa(A, Array{Float64,2}) + @test isa(B, Array{Float64,2}) + @test isa(C, Array{Float64,2}) + @test isa(D, Array{Float64,2}) + + @test D[1,1] == number + + @test size(A) == (0,0) + @test size(B) == (0,1) + @test size(C) == (1,0) + @test size(D) == (1,1) + end + + @testset "LTI models" begin + + # Fixture + Random.seed!(0); M=3; N=2; + SISO_tf = tf([1,0],[1,0,1]) + SISO_ss = ss(SISO_tf) + MIMO_ss = ss( + rand(Float64, (M,M)), + rand(Float64, (M,N)), + rand(Float64, (N,M)), + rand(Float64, (N,N)) + ) + MIMO_tf = tf(MIMO_ss) + + # check that conversion of SISO tf data is OK + A, B, C, D = ControlSystems._input2ss(SISO_tf) + @test isa(A, Array{Float64,2}) + @test isa(B, Array{Float64,2}) + @test isa(C, Array{Float64,2}) + @test isa(D, Array{Float64,2}) + + @test ss(A, B, C, D) == ss(SISO_tf) + + # check that conversion of SISO tf data is OK + A, B, C, D = ControlSystems._input2ss(SISO_ss) + @test isa(A, Array{Float64,2}) + @test isa(B, Array{Float64,2}) + @test isa(C, Array{Float64,2}) + @test isa(D, Array{Float64,2}) + + @test ss(A, B, C, D) == SISO_ss + + # check that conversion of MIMO tf data is OK + A, B, C, D = ControlSystems._input2ss(MIMO_tf) + @test isa(A, Array{Float64,2}) + @test isa(B, Array{Float64,2}) + @test isa(C, Array{Float64,2}) + @test isa(D, Array{Float64,2}) + + @test ss(A, B, C, D) == ss(MIMO_tf) + + # check that conversion of MIMO tf data is OK + A, B, C, D = ControlSystems._input2ss(MIMO_ss) + @test isa(A, Array{Float64,2}) + @test isa(B, Array{Float64,2}) + @test isa(C, Array{Float64,2}) + @test isa(D, Array{Float64,2}) + + @test ss(A, B, C, D) == MIMO_ss + end + end + + # + # @testset "Dimensionality checks" + # + # # Check sensitivity function weight - must have the same number of inputs + # # as outputs, can have an arbitrarily large statespace, and must have the + # # same number of inputs as there are outputs in the process which is to be + # # controlled. + # + # # Fixture + # M = 5; N = 3; L=4; + # G = ss(rand(Float64, (M,M)), rand(Float64, (M,L)), + # rand(Float64, (N,M)), rand(Float64, (N,L))) + # + # # Create a sensitivity weighting function with ii inputs and ii outputs + # for ii = 2:6 + # for jj = 2:6 + # for kk = 2:6 + # # Create some randome weighting functions + # WS = ss(rand(Float64, (M,M)), rand(Float64, (M,ii)), + # rand(Float64, (ii,M)), rand(Float64, (ii,ii))) + # WU = ss(rand(Float64, (M,M)), rand(Float64, (M,jj)), + # rand(Float64, (jj,M)), rand(Float64, (jj,jj))) + # WT = ss(rand(Float64, (M,M)), rand(Float64, (M,kk)), + # rand(Float64, (kk,M)), rand(Float64, (kk,kk))) + # + # println([ii,jj,kk]) + # # Chech that the specifications can be re-written is possible + # if ii == N && jj == L && kk == N + # @test isa(hInf_partition(G, WS, WU, WT), ControlSystems.ExtendedStateSpace) + # else + # @test_throws ErrorException hInf_partition(G, WS, WU, WT) + # end + # end + # end + # end + # end + end + end + + if execute_tests[2] + @testset "(2) Assumptions" begin + """ + Tests the methods used to check that the assumptions are satisfied, + incorporating two separate algorithms to determine detectability and + observability, and a rectangular pseudoinverse to check the rank conditions. + """ + + @testset "Stabilizability check" begin + """ + Test the check for stabilizability using the Hautus Lemma + """ + ### Fixture + Random.seed!(0); N = 10; M = 5; + R = rand(Float64, (N,N)) + Q = eigvecs(R + R'); + L = rand(Float64,N).-0.5; + A = Q*Diagonal(L)*Q'; + E = eigvals(A); + + # Here we should return false if for any positive eigenvalue λ of A, we get + # that rank(A-λI, B) < N. We define B as conlums which are linearly + # dependent with columns in A-λI, and check that the system returns false + # in every case where λ > 0 and never when λ≦0. + for ii = 1:N + for jj = 1:(N-M) + Ahat = A - E[ii]*Matrix{Float64}(I, N, N); + B = Ahat[:,jj:(jj+M)] + if E[ii] >= 0 + @test !ControlSystems._stabilizable(A,B) + else + @test ControlSystems._stabilizable(A,B) + end + end + end + + # Check common input types and ensure that a method error is thrown when + # not using two abstract matrices, but some other common type + N = 10; M = 5; + A = rand(Float64, (N,N)); + B = rand(Float64, (N,M)); + @test_throws MethodError ControlSystems._stabilizable(A,nothing) + @test_throws MethodError ControlSystems._stabilizable(nothing,B) + @test_throws MethodError ControlSystems._stabilizable(A,[]) + @test_throws MethodError ControlSystems._stabilizable([],B) + @test_throws MethodError ControlSystems._stabilizable(A,ss(1)) + @test_throws MethodError ControlSystems._stabilizable(ss(1),B) + @test_throws MethodError ControlSystems._stabilizable(A,tf(1)) + @test_throws MethodError ControlSystems._stabilizable(tf(1),B) + end + + @testset "Detectability check" begin + """ + Test the check for detectability using the Hautus Lemma + """ + ### Fixture + Random.seed!(0); N = 10; M = 5; + R = rand(Float64, (N,N)) + Q = eigvecs(R + R'); + L = rand(Float64,N).-0.5; + A = Q*Diagonal(L)*Q'; + E = eigvals(A); + + # Here we should return false if for any positive eigenvalue λ of A, we get + # that rank(A-λI; C) < N. We define C as rows which are linearly + # dependent with rows in A-λI, and check that the system returns false + # in every case where λ > 0 and never when λ≦0. + for ii = 1:N + for jj = 1:(N-M) + Ahat = A - E[ii]*Matrix{Float64}(I, N, N); + C = Ahat[jj:(jj+M),:] + if E[ii] >= 0 + @test !ControlSystems._detectable(A,C) + else + @test ControlSystems._detectable(A,C) + end + end + end + + # Check common input types and ensure that a method error is thrown when + # not using two abstract matrices, but some other common type + N = 10; M = 5; + A = rand(Float64, (M,M)); + C = rand(Float64, (N,M)); + @test_throws MethodError ControlSystems._detectable(A,nothing) + @test_throws MethodError ControlSystems._detectable(nothing,C) + @test_throws MethodError ControlSystems._detectable(A,[]) + @test_throws MethodError ControlSystems._detectable([],C) + @test_throws MethodError ControlSystems._detectable(A,ss(1)) + @test_throws MethodError ControlSystems._detectable(ss(1),C) + @test_throws MethodError ControlSystems._detectable(A,tf(1)) + @test_throws MethodError ControlSystems._detectable(tf(1),C) + end + + # TODO: write tests using the above submethods directly in hInf_assumptions + end + end + + if execute_tests[3] + @testset "(3) Coordinate transform" begin + """ + Computes the coordinate transfomration to write the system on the form of + assumption A4 in [1], i.e. take an extended statespace model, P, satisfying + assumption A2, and transform is to a system P_bar, where the matrices + D12_bar = [0,I] and D21_bar = [0;I]. + """ + + @testset "Find transformation for full rank matrix with QR" begin + """ + Test computation of the transformation in square and rectangular cases. + """ + # Fixture + tolerance = 1e-10 + + for N = 1:5 + for M = 1:5 + A = rand(Float64, (M, N)) + I_mat = Matrix{Float64}(I, min(M,N), min(M,N)) + + if M==N + # Square case + A_bar_true = I_mat + elseif M>N + # Rectangular case (more rows than columns) + Z_mat = zeros(Float64, (max(M,N)-min(M,N),min(M,N))) + A_bar_true = [Z_mat; I_mat] + else + # Rectangular case (more columns than rows) + Z_mat = zeros(Float64, (min(M,N), max(M,N)-min(M,N))) + A_bar_true = [Z_mat I_mat] + end + Tl,Tr = ControlSystems._coordinatetransformqr(A) + @test opnorm(Tl*A*Tr - A_bar_true) < tolerance + end + end + end + + @testset "Find transformation for full rank matrix with SVD" begin + """ + Test computation of the transformation in square and rectangular cases. + """ + # Fixture + tolerance = 1e-10 + + for N = 1:5 + for M = 1:5 + A = rand(Float64, (M, N)) + I_mat = Matrix{Float64}(I, min(M,N), min(M,N)) + + if M==N + # Square case + A_bar_true = I_mat + elseif M>N + # Rectangular case (more rows than columns) + Z_mat = zeros(Float64, (max(M,N)-min(M,N),min(M,N))) + A_bar_true = [Z_mat; I_mat] + else + # Rectangular case (more columns than rows) + Z_mat = zeros(Float64, (min(M,N), max(M,N)-min(M,N))) + A_bar_true = [Z_mat I_mat] + end + Tl,Tr = ControlSystems._coordinatetransformsvd(A) + @test opnorm(Tl*A*Tr - A_bar_true) < tolerance + end + end + end + + @testset "Find transformation for arbitrary matrix inputs" begin + """ + Test computation of the transformation in square and rectangular cases. + """ + # Fixture + test_matrices_full = [ + rand(Float64,(1,1)), + rand(Float64,(5,5)), + rand(Float64,(2,5)), + rand(Float64,(5,2)) + ] + + # Compute rank deficient matrices + test_matrices_rank_deficient = test_matrices_full + for ii = 1:length(test_matrices_full) + U,S,V = svd(test_matrices_full[ii]) + S[1]=0 + test_matrices_rank_deficient[ii] = U*Diagonal(S)*V' + end + + # Make sure that an exception is raised if assumption A2 is violated, + # any of the matrices which are to be decomposed are rank deficient + for A = test_matrices_rank_deficient + println(A) + @test_throws ErrorException ControlSystems._scalematrix(A) + end + + # Various bad inputs + @test_throws MethodError ControlSystems._scalematrix(tf(1)) + @test_throws MethodError ControlSystems._scalematrix(ss(1)) + @test_throws MethodError ControlSystems._scalematrix([]) + @test_throws MethodError ControlSystems._scalematrix(nothing) + @test_throws MethodError ControlSystems._scalematrix(1) + + # Check that errors are thrown if the matric has zise zero + @test_throws ErrorException ControlSystems._scalematrix(zeros(Float64,(0,1))) + + # Various bad methods + A = test_matrices_full[1]; + @test_throws ErrorException ControlSystems._scalematrix(A; method="bad method") + @test_throws ErrorException ControlSystems._scalematrix(A; method=1) + end + + @testset "Test application of the transformation to an ESS object" begin + """ + The wa + """ + end + end + end + + if execute_tests[4] + @testset "(4) Gamma iterations" begin + """ + Tests the core methods of the gamma-iteration bisection method, including + the ARE hamiltonian solver, the eigenvalue solver, the feasibility checks. + """ + + @testset "Solution feasibility check" begin + """ + Check that a solution to the dual riccati equations is correctly reported + as being feasible if satisfying the conditions of positive definiteness on + the X and Y solutions are met, and the spectral radius condition of X*Y is + also met + """ + # Fixture + Random.seed!(0); + tolerance = 1e-10; + iteration = 1; + Random.seed!(0); N = 10; M = 5; + R = rand(Float64, (N,N)) + Q = eigvecs(R + R'); + ρX = rand() + ρY = rand() + LX= rand(Float64,N); LX = ρX*sort(LX / maximum(LX)) + LY= rand(Float64,N); LY = ρY*sort(LY / maximum(LY)) + Xinf = Q*Diagonal(LX)*Q'; + Yinf = Q*Diagonal(LY)*Q'; + gamma = 1; + + # Test that the fesibility is true if ρ(Xinf*Yinf) < γ^2, that is the + # check should be true for any + # + # γ = sqrt(ρX*ρY) + ϵ + # + # with any epsilon greater than or equal to zero. + @test !ControlSystems._checkfeasibility(Xinf, Yinf, sqrt(ρX*ρY)-tolerance, tolerance, iteration; verbose=false) + @test !ControlSystems._checkfeasibility(Xinf, Yinf, sqrt(ρX*ρY), tolerance, iteration; verbose=false) + @test ControlSystems._checkfeasibility(Xinf, Yinf, sqrt(ρX*ρY)+tolerance, tolerance, iteration; verbose=false) + + # Test that errors are thrown if the matrix Xinf and Yinf are not PSD down + # to the numerical tolerance. + L = LX; + L[1] += -L[1] + 2*tolerance; Xpos = Q*Diagonal(L)*Q'; # slightly positive eigenvalue + L[1] += -L[1]; Xzero = Q*Diagonal(LX)*Q'; # exactly one zero eigenvalue + L[1] += -L[1] - 2*tolerance; Xneg = Q*Diagonal(L)*Q'; # slightly negative eigenvalue + @test ControlSystems._checkfeasibility(Xpos, Yinf, sqrt(ρX*ρY)+tolerance, tolerance, iteration; verbose=false) + @test ControlSystems._checkfeasibility(Xzero, Yinf, sqrt(ρX*ρY)+tolerance, tolerance, iteration; verbose=false) + @test !ControlSystems._checkfeasibility(Xneg, Yinf, sqrt(ρX*ρY)+tolerance, tolerance, iteration; verbose=false) + + L = LY; + L[1] += -L[1] + 2*tolerance; Ypos = Q*Diagonal(L)*Q'; # slightly positive eigenvalue + L[1] += -L[1]; Yzero = Q*Diagonal(L)*Q'; # exactly one zero eigenvalue + L[1] += -L[1] - 2*tolerance; Yneg = Q*Diagonal(L)*Q'; # slightly negative eigenvalue + @test ControlSystems._checkfeasibility(Xinf, Ypos, sqrt(ρX*ρY)+tolerance, tolerance, iteration; verbose=false) + @test ControlSystems._checkfeasibility(Xinf, Yzero, sqrt(ρX*ρY)+tolerance, tolerance, iteration; verbose=false) + @test !ControlSystems._checkfeasibility(Xinf, Yneg, sqrt(ρX*ρY)+tolerance, tolerance, iteration; verbose=false) + end + + # TODO: Include a check to verify that the bisection works as intended. + # TODO: Check to verify that the Hamiltonian Shur-solver is working + # TODO: Check to verify that the Hamiltonian Eigenvalue-solver is working + end + end + + if execute_tests[7] + @testset "(7) Bilinear discretization" begin + """ + This tests the bilinear method of discretizing a continuous time StateSpace + or ExtendedStateSpace object, moving from the Laplace-domain to the Z-domain. + However, importantly, the method also enables approximating discrete-time + systems with continuous-time state-space domain, moving from the Z-domain back + to the laplace-domain. Since the L∞-norm is invariant over the bilinear + discretization and "continuization", this effectively allows us to approximate + discrete-time systems with a continuous-time equivalent, design an H∞-optimal + controller for the continuous-time plant, and then re-discretizing this + controller. + + The structure of these tests is to simply discretize a set of plants with + various dimensions, transport them back into continuous-time domain, and then + to discrete-time again. By then comparing the resulting two pairs of discrete- + time and continuous-time systems in the frequency domain, we can test if the + bilinear discretization operates as expected. + """ + + @testset "SS data" begin + # Fixture for the SS-type data + Random.seed!(0); + N = [1,5,10] + M = [1,5,10] + H = [0.1,0.01,0.001] + tolerance = 1e-7; + + # Tests for the SS-type data + for (ii, m) in enumerate(N) + for (jj, n) in enumerate(M) + for (kk, h) in enumerate(H) + + freq = [10^i for i in range(-6, stop=log10(pi/h), length=101)] + + AcTrue, BcTrue, CcTrue, DcTrue = rand(m,m), rand(m,n), rand(n,m), rand(n,n) + + ev = eigvals(AcTrue) + if !isempty(ev[real(ev).<0]) + AcTrue = AcTrue - one(AcTrue) * 2 * maximum(abs.(real(ev[real(ev).<0]))) + end + + Gtrue = ss(AcTrue, BcTrue, CcTrue, DcTrue) + valA, fA = sigma(Gtrue, freq); + sysB = bilinearc2d(Gtrue, h); + valB, fB = sigma(sysB, freq) + sysC = bilineard2c(bilinearc2d(Gtrue, h)) + valC, fC = sigma(sysC, freq) + sysD = bilinearc2d(bilineard2c(bilinearc2d(Gtrue, h)),h) + valD, fD = sigma(sysD, freq) + + @test abs(maximum(svd(sysB.B).S)-maximum(svd(sysB.C).S)) < tolerance + @test abs(maximum(svd(sysC.B).S)-maximum(svd(sysC.C).S)) < tolerance + @test abs(maximum(svd(sysD.B).S)-maximum(svd(sysD.C).S)) < tolerance + + # Test that the C->D->C and D->C->D results in the same + @test norm(valA-valC, Inf) < tolerance + @test norm(valB-valD, Inf) < tolerance + end + end + end + end + + @testset "Extended SS data" begin + # Fixture for the extended SS-type data + Random.seed!(0); + N = [1,4] + P1 = [1,4] + P2 = [1,4] + M1 = [1,4] + M2 = [1,4] + H = [0.1,0.01,0.001] + tolerance = 1e-5; + + for (in1, n) in enumerate(N) + for (im1, m1) in enumerate(M1) + for (im2, m2) in enumerate(M2) + for (ip1, p1) in enumerate(P1) + for (ip2, p2) in enumerate(P2) + for (ih, h) in enumerate(H) + + freq = [10^i for i in range(-4, stop=log10(pi/h), length=101)] + + A, B1, B2 = 0.01*rand(n,n), rand(n,m1), rand(n,m2) + C1, D11, D12 = rand(p1,n), rand(p1,m1), rand(p1,m2) + C2, D21, D22 = rand(p2,n), rand(p2,m1), rand(p2,m2) + + ev = eigvals(A) + if !isempty(ev[real(ev).>0]) + A = A - one(A) * 2 * maximum(abs.(real(ev[real(ev).>0]))) + end + + EsysA = ss(A, B1, B2, C1, C2, D11, D12, D21, D22) + valA, fA = sigma(ss( + EsysA.A, + [EsysA.B1 EsysA.B2], + [EsysA.C1; EsysA.C2], + [EsysA.D11 EsysA.D12; EsysA.D21 EsysA.D22]) , freq) + + # To discrete time + EsysB = bilinearc2d(EsysA, h) + valB, fB = sigma(ss( + EsysB.A, + [EsysB.B1 EsysB.B2], + [EsysB.C1; EsysB.C2], + [EsysB.D11 EsysB.D12; EsysB.D21 EsysB.D22],h) , freq) + + # To continuous time + EsysC = bilineard2c(bilinearc2d(EsysA, h)) + valC, fC = sigma(ss( + EsysC.A, + [EsysC.B1 EsysC.B2], + [EsysC.C1; EsysC.C2], + [EsysC.D11 EsysC.D12; EsysC.D21 EsysC.D22]) , freq) + + # To discrete time + EsysD = bilinearc2d(bilineard2c(bilinearc2d(EsysA, h)),h) + valD, fD = sigma(ss( + EsysD.A, + [EsysD.B1 EsysD.B2], + [EsysD.C1; EsysD.C2], + [EsysD.D11 EsysD.D12; EsysD.D21 EsysD.D22],h) , freq) + + # Test that the C->D->C and D->C->D results in the same + @test norm(valA-valC, Inf) < tolerance + @test norm(valB-valD, Inf) < tolerance + end + end + end + end + end + end + end + end + end + + if execute_tests[9] + @testset "(9) Synthesis Examples" begin + + @testset "DC motor example" begin + # Fixture + tolerance = 1e-4 + + # Make sure that the code runs + @test isa(include("../example/hinf_example_DC.jl"), Nothing) + Ω = [10^i for i in range(-3, stop=3, length=201)] + + # Check that the optimal gain is correct + @test abs(gamma - 4.463211059) < tolerance + + # Check that the closed loop satisfies ||F_l(P(jω), C(jω)||_∞ < γ, ∀ω ∈ Ω + valPcl = sigma(Pcl, Ω)[1]; + @test all(valPcl .< (gamma+tolerance)) + + # Check that ||S(jω)/WS(jω)||_∞ < γ, ∀ω ∈ Ω + if isa(WS, LTISystem) || isa(WS, Number) + valSWS = sigma(S * WS , Ω)[1]; + @test all(valSWS .< (gamma+tolerance)) + end + + # Check that ||C(jω)S(jω)/W_U(jω)||_∞ < γ, ∀ω ∈ Ω + if isa(WU, LTISystem) || isa(WU, Number) + valKSWU = sigma(CS * WU , Ω)[1]; + @test all(valKSWU .< (gamma+tolerance)) + end + + # Check that ||T(jω)/W_T(jω)||_∞ < γ, ∀ω ∈ Ω + if isa(WT, LTISystem) || isa(WT, Number) + valTWT = sigma(T * WT , Ω)[1]; + @test all(valTWT .< (gamma+tolerance)) + end + end + + @testset "MIT open courseware example" begin + + # Fixture + tolerance = 1e-4 + + # Make sure that the code runs + @test isa(include("../example/hinf_example_MIT.jl"), Nothing) + Ω = [10^i for i in range(-7, stop=7, length=201)] + + # Check that the optimal gain is correct + @test abs(gamma - 0.923430124918) < tolerance + + # Check that the closed loop satisfies ||F_l(P(jω), C(jω)||_∞ < γ, ∀ω ∈ Ω + valPcl = sigma(Pcl, Ω)[1]; + @test all(valPcl .< (gamma+tolerance)) + + # Check that ||S(jω)/WS(jω)||_∞ < γ, ∀ω ∈ Ω + if isa(WS, LTISystem) || isa(WS, Number) + valSWS = sigma(S * WS , Ω)[1]; + @test all(valSWS .< (gamma+tolerance)) + end + + # Check that ||C(jω)S(jω)/W_U(jω)||_∞ < γ, ∀ω ∈ Ω + if isa(WU, LTISystem) || isa(WU, Number) + valKSWU = sigma(CS * WU , Ω)[1]; + @test all(valKSWU .< (gamma+tolerance)) + end + + # Check that ||T(jω)/W_T(jω)||_∞ < γ, ∀ω ∈ Ω + if isa(WT, LTISystem) || isa(WT, Number) + valTWT = sigma(T * WT , Ω)[1]; + @test all(valTWT .< (gamma+tolerance)) + end + end + + @testset "Quad tank example" begin + + # Fixture + tolerance = 1e-2 + + # Make sure that the code runs + @test isa(include("../example/hinf_example_tank.jl"), Nothing) + Ω = [10^i for i in range(-7, stop=7, length=201)] + + # Check that the optimal gain is correct + @test abs(gamma - 0.9534467) < tolerance + + # Check that the closed loop satisfies ||F_l(P(jω), C(jω)||_∞ < γ, ∀ω ∈ Ω + valPcl = sigma(Pcl, Ω)[1]; + @test all(valPcl[:,1] .< (gamma+tolerance)) + + # Check that ||S(jω)/WS(jω)||_∞ < γ, ∀ω ∈ Ω + if isa(WS, LTISystem) || isa(WS, Number) + valSWS = sigma(S * WS , Ω)[1]; + @test all(valSWS[:,1] .< (gamma+tolerance)) + end + + # Check that ||C(jω)S(jω)/W_U(jω)||_∞ < γ, ∀ω ∈ Ω + if isa(WU, LTISystem) || isa(WU, Number) + valKSWU = sigma(CS * WU , Ω)[1]; + @test all(valKSWU[:,1] .< (gamma+tolerance)) + end + + # Check that ||T(jω)/W_T(jω)||_∞ < γ, ∀ω ∈ Ω + if isa(WT, LTISystem) || isa(WT, Number) + valTWT = sigma(T * WT , Ω)[1]; + @test all(valTWT[:,1] .< (gamma+tolerance)) + end + end + end + end +end