From a5d6e849e1d13afdf9259d302dbd0b8bd0d34e28 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Fri, 11 Feb 2022 13:47:54 +0100 Subject: [PATCH 1/6] improvements to h2 design --- src/ExtendedStateSpace.jl | 5 ++++ src/h2_design.jl | 45 +++++++++++++++++++++----------- src/lqg.jl | 55 ++++++++++++++++++++++++++++++--------- src/plotting.jl | 9 +++++-- test/test_h2_design.jl | 47 ++++++++++++++++++++++++++++++++- 5 files changed, 131 insertions(+), 30 deletions(-) diff --git a/src/ExtendedStateSpace.jl b/src/ExtendedStateSpace.jl index 9710320c..152502a3 100644 --- a/src/ExtendedStateSpace.jl +++ b/src/ExtendedStateSpace.jl @@ -179,8 +179,13 @@ function Base.getproperty(sys::ExtendedStateSpace, s::Symbol) end end +Base.propertynames(sys::ExtendedStateSpace) = (:A, :B, :C, :D, :B1, :B2, :C1, :C2, :D11, :D12, :D21, :D22, :Ts, :timeevol, :nx, :ny, :nu, :nw, :nz, :zinds, :yinds, :winds, :uinds) + ControlSystems.StateSpace(s::ExtendedStateSpace) = ss(ssdata(s)..., s.timeevol) +""" + A, B1, B2, C1, C2, D11, D12, D21, D22 = ssdata_e(sys) +""" ssdata_e(sys::ExtendedStateSpace) = sys.A, sys.B1, sys.B2, diff --git a/src/h2_design.jl b/src/h2_design.jl index aa2e408d..ea829187 100644 --- a/src/h2_design.jl +++ b/src/h2_design.jl @@ -10,12 +10,18 @@ If `γ = nothing`, use the formulas for H₂ in Ch 14.5. If γ is a large value, function h2synthesize(P::ExtendedStateSpace, γ = nothing) if γ === nothing + iszero(P.D11) && iszero(P.D22) || error("D11 and D22 must be zero for the standard H2 formulation to be used, try calling with γ=1000 to use the H∞ formulation.") X2, Y2, F2, L2 = _solvematrixequations2(P) Â2 = P.A + P.B2*F2 + L2*P.C2 - K = ss(Â2, -L2, F2, 0) + # Af2 = P.A + P.B2*F2 + # C1f2 = P.C1 + P.D12*F2 + # Gc = ss(Af2, I(size(Af2, 1)), C1f2, 0) + K = ss(Â2, -L2, F2, 0, P.timeevol) return K, lft(ss(P), K) end + iscontinuous(P) || throw(ArgumentError("h2syn with specified γ is only supported for continuous systems.")) + P̄, Ltrans12, Rtrans12, Ltrans21, Rtrans21 = _transformp2pbar(P) X2, Y2, F2, L2 = _solvematrixequations(P̄, γ) @@ -52,26 +58,35 @@ function _solvematrixequations2(P::ExtendedStateSpace) 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) + # P1 = size(C1, 1) + # P2 = size(C2, 1) + # M1 = size(B1, 2) + # M2 = size(B2, 2) + + # HX = [A zeros(size(A)); -C1'*C1 -A'] - [B2; -C1'*D12] * [D12'*C1 B2'] + # HY = [A' zeros(size(A)); -B1*B1' -A] - [C2'; -B1*D21'] * [D21*B1' C2] + + # # Solve matrix equations + # X2 = _solvehamiltonianare(HX) + # Y2 = _solvehamiltonianare(HY) + + # # These formulas appear under ch14.5, but different formulas where D12'C1=0 and B1 * D21'=0 appear + # # in ch 14.9.1, the difference is explained in ch13. See also LQGProblem(P::ExtendedStateSpace) + # F2 = (B2'X2 + D12'C1) + # L2 = (B1 * D21' + Y2 * C2') - HX = [A zeros(size(A)); -C1'*C1 -A'] - [B2; -C1'*D12] * [D12'*C1 B2'] - HY = [A' zeros(size(A)); -B1*B1' -A] - [C2'; -B1*D21'] * [D21*B1' C2] - # Solve matrix equations - X2 = _solvehamiltonianare(HX) - Y2 = _solvehamiltonianare(HY) + # This is more robust than the approach above + # https://ocw.snu.ac.kr/sites/default/files/NOTE/3938.pdf eq 73-74 - F2 = -(B2'X2 + D12'C1) + fun = iscontinuous(P) ? MatrixEquations.arec : MatrixEquations.ared - L2 = -(B1 * D21' + Y2 * C2') + X2, _, F2 = fun(A, B2, D12'D12, C1'C1, C1'D12) + Y2, _, L2 = fun(A', C2', D21*D21', B1*B1', B1*D21') + L2 = L2' - return X2, Y2, F2, L2 + return X2, Y2, -F2, -L2 end \ No newline at end of file diff --git a/src/lqg.jl b/src/lqg.jl index b53893c7..a9592784 100644 --- a/src/lqg.jl +++ b/src/lqg.jl @@ -64,6 +64,8 @@ struct LQGProblem R2::AbstractMatrix qQ::Real qR::Real + SQ::AbstractMatrix + SR::AbstractMatrix end ControlSystems.isdiscrete(l::LQGProblem) = ControlSystems.isdiscrete(l.sys) @@ -77,6 +79,8 @@ function LQGProblem( R2::AbstractVecOrMat; qQ = 0, qR = 0, + SQ = nothing, + SR = nothing, kwargs..., ) sys isa ExtendedStateSpace || (sys = ExtendedStateSpace(sys, B1=I, C1=I)) @@ -88,52 +92,79 @@ function LQGProblem( size(Q1, 1) == size(C1,1) || throw(ArgumentError("The size of Q1 is determined by C1, not by the state.")) size(R2, 1) == size(C2,1) || throw(ArgumentError("The size of R2 is determined by C2, not by the state.")) size(R1, 1) == size(B1,2) || throw(ArgumentError("The size of R1 is determined by B1, not by the state.")) - LQGProblem(sys, Q1, Q2, R1, R2, qQ, qR; kwargs...) + SQ === nothing && (SQ = zeros(size(sys.B2))) + SR === nothing && (SR = zeros(size(sys.C2'))) + + LQGProblem(sys, Q1, Q2, R1, R2, qQ, qR, SQ, SR; kwargs...) end """ LQGProblem(P::ExtendedStateSpace) -If only an `ExtendedStateSpace` system is provided, the system `P` is assumed to correspond to the H₂ optimal control problem with +If only an `ExtendedStateSpace` system is provided, e.g. from `hinfpartition`, the system `P` is assumed to correspond to the H₂ optimal control problem with ``` C1'C1 = Q1 D12'D12 = Q2 +SQ = C1'D12 # Cross term B1*B1' = R1 D21*D21' = R2 +SR = B1*D21' # Cross term ``` and an `LQGProblem` with the above covariance matrices is returned. The system description in the returned LQGProblem will have `B1 = C1 = I`. -See Ch. 13 in Robust and optimal control for reference. +See Ch. 14 in Robust and optimal control for reference. + +# Example: +All the following ways of obtaining the H2 optimal controller are (almost) equivalent +```julia +using Test +G = ss(tf(1, [10, 1])) +WS = tf(1, [1, 1e-6]) +WU = makeweight(1e-2, 0.1, 100) +Gd = hinfpartition(G, WS, WU, []) + +K, Gcl = h2synthesize(Gd) # First option, using H2 formulas +K2, Gcl2 = h2synthesize(Gd, 1000) # Second option, using H∞ formulas with large γ + +lqg = LQGProblem(Gd) # Third option, convert to an LQGProblem and obtain controller +K3 = -observer_controller(lqg) + +@test h2norm(lft(Gd, K )) ≈ 3.0568 atol=1e-3 +@test h2norm(lft(Gd, K2)) ≈ 3.0568 atol=1e-3 +@test h2norm(lft(Gd, K3)) ≈ 3.0568 atol=1e-3 +``` """ function LQGProblem(P::ExtendedStateSpace) @unpack A, B1, B2, C1, C2, D11, D12, D21, D22 = P # all(iszero, D22) || throw(ArgumentError("Non-zero D22 not handled.")) all(iszero, D11) || throw(ArgumentError("Non-zero D11 not handled.")) - all(iszero, D12'C1) || throw(ArgumentError("D12'C1 should be 0")) - all(iszero, D21*B1') || throw(ArgumentError("D21*B1' should be 0")) + # all(iszero, D12'C1) || throw(ArgumentError("D12'C1 should be 0")) # One could get around this if using the cross-term in ARE, see + # all(iszero, D21*B1') || throw(ArgumentError("D21*B1' should be 0")) Q1 = C1'C1 Q2 = D12'D12 R1 = B1*B1' R2 = D21*D21' + SR = B1*D21' + SQ = C1'D12 B1 = I(P.nx) C1 = I(P.nx) P = ss(A, B1, B2, C1, C2; D22, Ts = P.timeevol) # P = ss(A, B1, B2, C1, C2; Ts = P.timeevol) - LQGProblem(P, Q1, Q2, R1, R2) + LQGProblem(P, Q1, Q2, R1, R2; SQ, SR) end function ControlSystems.kalman(l::LQGProblem) - @unpack A, C2, B1, R1, qR, B2, R2 = l - fun = isdiscrete(l) ? dkalman : kalman - K = fun(A, C2, Hermitian(B1*R1*B1' + qR * B2 * B2'), R2) + @unpack A, C2, B1, R1, qR, B2, R2, SR = l + fun = isdiscrete(l) ? ControlSystems.dkalman : kalman + K = fun(A, C2, Hermitian(B1*R1*B1' + qR * B2 * B2'), R2, SR) end function ControlSystems.lqr(l::LQGProblem) - @unpack A, B2, C1, Q1, qQ, C2, Q2 = l - fun = isdiscrete(l) ? dlqr : lqr - L = fun(A, B2, Hermitian(C1'Q1*C1 + qQ * C2'C2), Q2) + @unpack A, B2, C1, Q1, qQ, C2, Q2, SQ = l + fun = isdiscrete(l) ? ControlSystems.dlqr : lqr + L = fun(A, B2, Hermitian(C1'Q1*C1 + qQ * C2'C2), Q2, SQ) end """ diff --git a/src/plotting.jl b/src/plotting.jl index b94cc015..80851ecb 100644 --- a/src/plotting.jl +++ b/src/plotting.jl @@ -31,8 +31,13 @@ specificationplot ) sensitivityfunctions = p.args[1] + if length(p.args) >= 2 + weightfunctions = p.args[2] + end if length(p.args) >= 3 - weightfunctions, γ = p.args[2:3] + γ = p.args[3] + else + γ = 1 end title --> "Specification sigma plot" @@ -59,7 +64,7 @@ specificationplot end end end - if length(p.args) >= 3 + if length(p.args) >= 2 ## Plot the weight functions for (index, W) in enumerate(weightfunctions) diff --git a/test/test_h2_design.jl b/test/test_h2_design.jl index 32eb958b..b44f98de 100644 --- a/test/test_h2_design.jl +++ b/test/test_h2_design.jl @@ -87,4 +87,49 @@ l = LQGProblem(P) @test lqr(l) == L @test kalman(l) == Kal -# Implement normalized coprime fact from sec 13.8 \ No newline at end of file +# Implement normalized coprime fact from sec 13.8 + + + +## Another test case +G = ss(tf(1, [10, 1])) +WS = tf(1, [1, 1e-6]) +WU = makeweight(1e-2, 0.1, 100) +Gd = hinfpartition(G, WS, WU, []) + +K, Gcl = h2synthesize(Gd) +K2, Gcl2 = h2synthesize(Gd, 1000) + +lqg = LQGProblem(Gd) +K3 = -observer_controller(lqg) + + +@test h2norm(lft(Gd, K)) ≈ 3.0568 atol=1e-3 +@test h2norm(lft(Gd, K2)) ≈ 3.0568 atol=1e-3 +@test h2norm(lft(Gd, K3)) ≈ 3.0568 atol=1e-3 + +# Same as above but discrete +Ts = 0.01 +disc(G) = c2d(ss(G), Ts) + +G = ss(tf(1, [10, 1])) +WS = tf(1, [1, 1e-6]) +WU = makeweight(1e-2, 0.1, 100) +G,WS,WU = disc.((G,WS,WU)) + +Gd = hinfpartition(G, WS, WU, []) +@test isdiscrete(Gd) +K, Gcl = h2synthesize(Gd) + +@test isdiscrete(K) +@test isdiscrete(Gcl) + +lqg = LQGProblem(Gd) +K3 = -observer_controller(lqg) +@test isdiscrete(K3) + + +@test norm(lft(Gd, K)) ≈ 0.3083 atol=1e-3 +@test norm(lft(Gd, K3)) ≈ 0.3083 atol=1e-3 + +# bodeplot([K, K3], w) \ No newline at end of file From 4f42b365e9793c21258393f83becff8e94e4d5ac Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Fri, 11 Feb 2022 14:36:30 +0100 Subject: [PATCH 2/6] avoid reducing state dimension in lqg functions --- src/lqg.jl | 4 ++-- test/test_h2_design.jl | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/lqg.jl b/src/lqg.jl index a9592784..ad622d32 100644 --- a/src/lqg.jl +++ b/src/lqg.jl @@ -272,7 +272,7 @@ Returns an expression for the feedback controller `u = Cy` that is obtained when Note: the transfer function returned is only a representation of the controller in the simple setting described above, e.g., it is not valid if the actual input contains anything that is not produced by a pure feedback from observed states. To obtain a controller that takes references into account, see `extended_controller`. """ function ControlSystems.observer_controller(l::LQGProblem, L::AbstractMatrix = lqr(l), K::AbstractMatrix = kalman(l)) - A,B,C,D = ssdata(system_mapping(l)) + A,B,C,D = ssdata(system_mapping(l, identity)) Ac = A - B*L - K*C + K*D*L # 8.26b Bc = K Cc = L @@ -281,7 +281,7 @@ function ControlSystems.observer_controller(l::LQGProblem, L::AbstractMatrix = l end function ff_controller(l::LQGProblem, L = lqr(l), K = kalman(l)) - Ae,Be,Ce,De = ssdata(system_mapping(l)) + Ae,Be,Ce,De = ssdata(system_mapping(l, identity)) Ac = Ae - Be*L - K*Ce + K*De*L # 8.26c Bc = Be*static_gain_compensation(l, L) Cc = L diff --git a/test/test_h2_design.jl b/test/test_h2_design.jl index b44f98de..53463bcb 100644 --- a/test/test_h2_design.jl +++ b/test/test_h2_design.jl @@ -104,9 +104,9 @@ lqg = LQGProblem(Gd) K3 = -observer_controller(lqg) -@test h2norm(lft(Gd, K)) ≈ 3.0568 atol=1e-3 -@test h2norm(lft(Gd, K2)) ≈ 3.0568 atol=1e-3 -@test h2norm(lft(Gd, K3)) ≈ 3.0568 atol=1e-3 +@test norm(lft(Gd, K)) ≈ 3.0568 atol=1e-3 +@test norm(lft(Gd, K2)) ≈ 3.0568 atol=1e-3 +@test norm(lft(Gd, K3)) ≈ 3.0568 atol=1e-3 # Same as above but discrete Ts = 0.01 From 969748549509b10600e2547187776913f2dd6e54 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Fri, 11 Feb 2022 14:54:44 +0100 Subject: [PATCH 3/6] clarify closedloop definition for lqg --- Project.toml | 2 +- src/glover_mcfarlane.jl | 4 ++-- src/lqg.jl | 26 ++++++++++---------------- test/test_lqg.jl | 6 +++--- 4 files changed, 16 insertions(+), 22 deletions(-) diff --git a/Project.toml b/Project.toml index b55b8a14..1690412b 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "RobustAndOptimalControl" uuid = "21fd56a4-db03-40ee-82ee-a87907bee541" authors = ["Fredrik Bagge Carlson", "Marcus Greiff"] -version = "0.3.0" +version = "0.3.1" [deps] ChainRulesCore = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" diff --git a/src/glover_mcfarlane.jl b/src/glover_mcfarlane.jl index 62af59e1..11023bf1 100644 --- a/src/glover_mcfarlane.jl +++ b/src/glover_mcfarlane.jl @@ -154,8 +154,8 @@ end For discrete systems, the `info` tuple contains also feedback gains `F, L` and observer gain `Hkf` such that the controller on observer form is given by ```math -x^+ = Ax + Bu + H_{kf}*(Cx - y)\\\\ -u = Fx + L*(Cx - y) +x^+ = Ax + Bu + H_{kf} (Cx - y)\\\\ +u = Fx + L (Cx - y) ``` Note, this controller is *not* strictly proper, i.e., it has a non-zero D matrix. The controller can be transformed to observer form for the scaled plant (`info.Gs`) diff --git a/src/lqg.jl b/src/lqg.jl index ad622d32..faf67f47 100644 --- a/src/lqg.jl +++ b/src/lqg.jl @@ -292,34 +292,28 @@ end """ closedloop(l::LQGProblem, L = lqr(l), K = kalman(l)) -Closed-loop system as defined in Glad and Ljung eq. 8.28 +Closed-loop system as defined in Glad and Ljung eq. 8.28. Note, this definition of closed loop is not the same as lft(P, K), which has B1 isntead of B2 as input matrix. Use `lft(l)` to get the system from disturbances to controlled variables `w -> z`. The return value will be the closed loop from reference only, other disturbance signals (B1) are ignored. See `feedback` for a more advanced option. + +Use `static_gain_compensation` to adjust the gain from references acting on the input B2, `dcgain(closedloop(l))*static_gain_compensation(l) ≈ I` """ function closedloop(l::LQGProblem, L = lqr(l), K = kalman(l)) # todo: reimplement as lft P = system_mapping(l, identity) - @unpack A, B2, C2, C1 = l + @unpack A, B1, B2, C2, C1 = l n = P.nx - # Lr = pinv(C1 * ((P.B * L[:, 1:n] - P.A) \ P.B)) - Lr = static_gain_compensation(l, L[:, 1:n]) - # Lr = (D - (C - D*L) * inv(A - B*L) * B) - if any(!isfinite, Lr) || all(iszero, Lr) - @warn "Could not compensate for static gain automatically." Lr - Lr = 1 - end Acl = [A-B2*L B2*L; zero(A) A-K*C2] # 8.28 - BLr = B2 * Lr # QUESTION: should be B1 here? Glad Ljung has B2 - Bcl = [BLr; zero(BLr)] + #Glad Ljung has B2 here instead of B1. The difference lies in Glad, Ljung calling the system from references acting through B2 the closed loop, whereas most other literature uses lft(P, K) as the closed loop, i.e., from B1 + Bcl = [B2; zero(B2)] + Ccl = [C1 zero(C1)] syscl = ss(Acl, Bcl, Ccl, 0, l.timeevol) end -# function closedloop(l::LQGProblem) -# K = observer_controller(l) -# Ke = extended_controller(K) -# lft(l.sys, Ke) -# end +ControlSystems.lft(l::LQGProblem) = lft(l.sys, -observer_controller(l)) + + system_mapping(l::LQGProblem, args...) = system_mapping(l.sys, args...) diff --git a/test/test_lqg.jl b/test/test_lqg.jl index f4f6e66e..ff4e2b1b 100644 --- a/test/test_lqg.jl +++ b/test/test_lqg.jl @@ -117,7 +117,7 @@ G = LQGProblem(sys, Q1, Q2, R1, R2) @test lqr(G) ≈ [-0.0036 0.39 0.21 3.11 0.63 1.54 0.046 -0.0073 0.085 -0.78 0.57 -3.10 0.20 0.30] rtol=0.01 -@test dcgain(RobustAndOptimalControl.closedloop(G)) ≈ I +@test dcgain(RobustAndOptimalControl.closedloop(G))*static_gain_compensation(G) ≈ I @@ -176,7 +176,7 @@ G = LQGProblem(sys, Q1, Q2, R1, R2, qQ = 0.000001) 99.9634 -0.0026 -0.8533] rtol=1e-2 @test lqr(G) ≈ [2.8106 1.4071 6.8099 3.7874 0] rtol=1e-2 -@test dcgain(RobustAndOptimalControl.closedloop(G))[] ≈ 1 +@test dcgain(RobustAndOptimalControl.closedloop(G)*static_gain_compensation(G))[] ≈ 1 ## example 9.3 @@ -225,7 +225,7 @@ R2 = 0.001I(2) # 0 31.62 # ] rtol = 0.01 -# @test dcgain(closedloop(G)) ≈ I +# @test dcgain(closedloop(G)*static_gain_compensation(G)) ≈ I # Random system, compare with ML output From 52c73fa031823a134d087235f955ed33fd3c3158 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Fri, 11 Feb 2022 16:32:20 +0100 Subject: [PATCH 4/6] proper handling of timeevol --- examples/hinf_example_tank.jl | 24 +++++++----------------- src/hinfinity_design.jl | 11 ++++++++--- 2 files changed, 15 insertions(+), 20 deletions(-) diff --git a/examples/hinf_example_tank.jl b/examples/hinf_example_tank.jl index 31eadd9f..ee435536 100644 --- a/examples/hinf_example_tank.jl +++ b/examples/hinf_example_tank.jl @@ -2,14 +2,10 @@ using ControlSystems, RobustAndOptimalControl 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) +This is a simple SISO example with integrator dynamics corresponding to a +quad tank process. """ -makeplots = true + # Define the proces parameters k1, k2, kc, g = 3.33, 3.35, 0.5, 981 @@ -57,16 +53,10 @@ C, γ = 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], [WS[1,1], 0.01, WT[1,1]], γ) +specificationplot([S, CS, T], [WS[1,1], 0.01, WT[1,1]], γ) ## Plot the closed loop gain from w to z -# TODO figure out why the legends don't seem to work in this case - - specificationplot(Pcl, γ; s_labels=["\$\\sigma(P_{cl}(j\\omega))\$"], w_labels=["\$\\gamma\$"]) +specificationplot(Pcl, γ; s_labels=["\$\\sigma(P_{cl}(j\\omega))\$"], w_labels=["\$\\gamma\$"]) - times = [i for i in range(0, stop=300, length=10000)] - plot(step(T, times)) -end +times = [i for i in range(0, stop=300, length=10000)] +plot(step(T, times)) diff --git a/src/hinfinity_design.jl b/src/hinfinity_design.jl index a95c419d..c5d984f9 100644 --- a/src/hinfinity_design.jl +++ b/src/hinfinity_design.jl @@ -447,11 +447,12 @@ function _γiterations( ) T = typeof(P.A) + ET = eltype(T) XinfFeasible, YinfFeasible, FinfFeasible, HinfFeasible, gammFeasible = T(undef,0,0), T(undef,0,0), T(undef,0,0), T(undef,0,0), nothing - gl, gu = interval - gl = max(1e-3, gl) + gl, gu = ET.(interval) + gl = max(ET(1e-3), gl) iters = ceil(Int, log2((gu-gl+1e-16)/gtol)) for iteration = 1:iters @@ -594,6 +595,10 @@ can be both MIMO and SISO, both in tf and ss forms). Valid inputs for the weighting functions are empty arrays, numbers (static gains), and `LTISystem`s. """ function hinfpartition(G, WS, WU, WT) + WS isa LTISystem && common_timeevol(G,WS) + WU isa LTISystem && common_timeevol(G,WU) + WT isa LTISystem && common_timeevol(G,WT) + te = G.timeevol # # Convert the systems into state-space form Ag, Bg, Cg, Dg = _input2ss(G) Asw, Bsw, Csw, Dsw = _input2ss(WS) @@ -726,7 +731,7 @@ function hinfpartition(G, WS, WU, WT) Dyw = Matrix{Float64}(I, mCg, nDuw) Dyu = -Dg - P = ss(A, Bw, Bu, Cz, Cy, Dzw, Dzu, Dyw, Dyu) + P = ss(A, Bw, Bu, Cz, Cy, Dzw, Dzu, Dyw, Dyu, te) end From f3eecbbe3f061deed3e11e24a4d02b61516b8373 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Sun, 13 Feb 2022 10:42:00 +0100 Subject: [PATCH 5/6] lu -> svd --- src/hinfinity_design.jl | 30 +++++++++++++++++++++++++++--- 1 file changed, 27 insertions(+), 3 deletions(-) diff --git a/src/hinfinity_design.jl b/src/hinfinity_design.jl index c5d984f9..0843b3b3 100644 --- a/src/hinfinity_design.jl +++ b/src/hinfinity_design.jl @@ -254,7 +254,8 @@ function _synthesizecontroller( D1122 = D11[(P1-M2+1):P1, (M1-P2+1):M1] # Equation 19 - D11hat = ((-D1121 * D1111') / (γ² * I - D1111 * D1111')) * D1112 - D1122 + J1 = (γ² * I - D1111 * D1111') + D11hat = ((-D1121 * D1111') / J1) * D1112 - D1122 # Equation 20 D12hatD12hat = I - (D1121 / (γ² * I - D1111' * D1111)) * D1121' @@ -262,7 +263,7 @@ function _synthesizecontroller( D12hat = cholesky(Hermitian(D12hatD12hat)).L # Equation 21 - D21hatD21hat = I - (D1112' / (γ² * I - D1111 * D1111')) * D1112 + D21hatD21hat = I - (D1112' / J1) * D1112 _assertrealandpsd(D21hatD21hat; msg = " in equation (21)") D21hat = cholesky(Hermitian(D21hatD21hat)).U @@ -306,6 +307,29 @@ function _synthesizecontroller( return ss(Ac, Bc[:, 1:P2], Cc[1:M2, :], D11c) end +""" + rqr(D, γ=1) + +"Regularized" qr factorization. This struct represents \$(D'D + γI)\$ without forming \$D'D\$ +Note: this does not support negative γ or \$(γI - D'D)\$ +Supported operations: `\\,/,*`, i.e., it behaves also like a matrix (unlike the standard `QR` factorization object). +""" +struct rqr{T, PT, DGT} + P::PT + DG::DGT + function rqr(D, γ=1) + P = (D'D) + γ*I + DG = qr([D; √(γ)I]) + new{eltype(P), typeof(P), typeof(DG)}(P, DG) + end +end + +Base.:\(d::rqr, b) = (d.DG.R\(adjoint(d.DG.R)\b)) +Base.:/(b, d::rqr) = ((b/d.DG.R)/adjoint(d.DG.R)) +Base.:*(d::rqr, b) = (d.P*b) +Base.:*(b, d::rqr) = (b*d.P) + + """ _assertrealandpsd(A::AbstractMatrix, msg::AbstractString) @@ -374,7 +398,7 @@ function _solvehamiltonianare(H)#::AbstractMatrix{<:LinearAlgebra.BlasFloat}) 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 + return U21 * pinv(U11) end """ From b52cd7bd494948de91f05b694051e418c1db4190 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Mon, 14 Feb 2022 15:57:00 +0100 Subject: [PATCH 6/6] add tests for rqr --- test/test_extendedstatespace.jl | 2 +- test/test_hinf_design.jl | 34 +++++++++++++++++++++++++++++++++ 2 files changed, 35 insertions(+), 1 deletion(-) diff --git a/test/test_extendedstatespace.jl b/test/test_extendedstatespace.jl index 15b93817..403a009b 100644 --- a/test/test_extendedstatespace.jl +++ b/test/test_extendedstatespace.jl @@ -54,7 +54,7 @@ w1 = :r^3 connections = [K.y .=> G.u; G.y .=> G.y; K.u .=> K.u] Gcl2 = connect([G, K, S], connections; z1, w1) -@test linfnorm(minreal(Gcl1 - Gcl2.sys))[1] < 1e-10 +@test linfnorm(minreal(Gcl1 - Gcl2.sys))[1] < sqrt(eps()) ## diff --git a/test/test_hinf_design.jl b/test/test_hinf_design.jl index 55c129c6..7f058bfe 100644 --- a/test/test_hinf_design.jl +++ b/test/test_hinf_design.jl @@ -5,6 +5,40 @@ using LinearAlgebra using Random +using RobustAndOptimalControl: rqr + +@testset "rqr" begin + @info "Testing rqr" + + e = sqrt(eps()) + D = [1 1; e 0; 0 e] + γ = 0.00001 + b = randn(2) + + bD = big.(D) + bb = big.(b) + hp = (bD'bD + big(γ)*I)\bb + + @test norm(rqr(D, γ)\b - hp) < 1e-10 + @test norm(rqr(D, γ)\b - hp) < norm((D'D + γ*I)\b - hp) + + @test rqr(D, γ)*b ≈ (D'D + γ*I)*b + + + γ = 0.00001 + b = randn(3,2) + bb = big.(b) + hp = bb/(bD'bD + big(γ)*I) + + @test norm(b/rqr(D, γ) - hp) < 1e-10 + @test norm(b/rqr(D, γ) - hp) < norm(b/(D'D + γ*I) - hp) + + @test b*rqr(D, γ) ≈ b*(D'D + γ*I) + + display(rqr(D, γ)) +end + + """ Tests for the public and private methods of the hInfSynthesis function. This function utilizes the preexisting ControlSystems toolbox, and performs a