Merge pull request #36 from JuliaControl/h2syn

improvements to H2 design

Author: baggepinnen

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))

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,

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`)

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

src/hinfinity_design.jl

@@ -254,15 +254,16 @@ 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'
     _assertrealandpsd(D12hatD12hat; msg = " in equation (20)")
     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
 
 """
@@ -447,11 +471,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 +619,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 +755,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