From 26b82654eb7322d7e2a7179da997e02416043185 Mon Sep 17 00:00:00 2001
From: Fredrik Bagge Carlson <baggepinnen@gmail.com>
Date: Wed, 12 Jan 2022 09:25:54 +0100
Subject: [PATCH] (actually) use hessenberg factorization for statespace
 freqresp

---
 src/freqresp.jl              | 62 ++++++++++++++++++++----------------
 test/test_delayed_systems.jl |  2 +-
 test/test_freqresp.jl        | 31 ++++++++++++++++++
 3 files changed, 66 insertions(+), 29 deletions(-)

diff --git a/src/freqresp.jl b/src/freqresp.jl
index 719097acf..bcd1351a7 100644
--- a/src/freqresp.jl
+++ b/src/freqresp.jl
@@ -8,44 +8,50 @@ function freqresp(sys::LTISystem, w::Real)
     evalfr(sys, s)
 end
 
-"""sys_fr = freqresp(sys, w)
+"""
+    sys_fr = freqresp(sys, w)
 
 Evaluate the frequency response of a linear system
 
 `w -> C*((iw*im -A)^-1)*B + D`
 
-of system `sys` over the frequency vector `w`."""
+of system `sys` over the frequency vector `w`.
+"""
 @autovec () function freqresp(sys::LTISystem, w_vec::AbstractVector{<:Real})
-    # Create imaginary freq vector s
-    if iscontinuous(sys)
-        s_vec = im*w_vec
-    else
-        s_vec = cis.(w_vec*(sys.Ts))
-    end
-    #if isa(sys, StateSpace)
-    #    sys = _preprocess_for_freqresp(sys)
-    #end
+    te = sys.timeevol
     ny,nu = noutputs(sys), ninputs(sys)
-    [evalfr(sys[i,j], s)[] for s in s_vec, i in 1:ny, j in 1:nu]
+    [evalfr(sys[i,j], _freq(w, te))[] for w in w_vec, i in 1:ny, j in 1:nu]
 end
 
-# Implements algorithm found in:
-# Laub, A.J., "Efficient Multivariable Frequency Response Computations",
-# IEEE Transactions on Automatic Control, AC-26 (1981), pp. 407-408.
-function _preprocess_for_freqresp(sys::StateSpace)
-    if isempty(sys.A) # hessfact does not work for empty matrices
-        return sys
+_freq(w, ::Continuous) = complex(0, w)
+_freq(w, te::Discrete) = cis(w*te.Ts)
+
+function freqresp(sys::AbstractStateSpace, w_vec::AbstractVector{W}) where W <: Real
+    ny, nu = size(sys)
+    T = promote_type(Complex{real(eltype(sys.A))}, Complex{W})
+    if sys.nx == 0 # Only D-matrix
+        return PermutedDimsArray(repeat(T.(sys.D), 1, 1, length(w_vec)), (3,1,2))
+    end
+    F = hessenberg(sys.A)
+    Q = Matrix(F.Q)
+    A = F.H
+    C = sys.C*Q
+    B = Q\sys.B 
+    D = sys.D
+
+    te = sys.timeevol
+    R = Array{T, 3}(undef, ny, nu, length(w_vec))
+    Bc = similar(B, T) # for storage
+    for i in eachindex(w_vec)
+        Ri = @views R[:,:,i]
+        copyto!(Ri,D) # start with the D-matrix
+        isinf(w_vec[i]) && continue
+        copyto!(Bc,B) # initialize storage to B
+        w = -_freq(w_vec[i], te)
+        ldiv!(A, Bc, shift = w) # B += (A - w*I)\B # solve (A-wI)X = B, storing result in B
+        mul!(Ri, C, Bc, -1, 1) # use of 5-arg mul to subtract from D already in Ri. - rather than + since (A - w*I) instead of (w*I - A)
     end
-    Tsys = numeric_type(sys)
-    TT = promote_type(typeof(zero(Tsys)/norm(one(Tsys))), Float32)
-
-    A, B, C, D = sys.A, sys.B, sys.C, sys.D
-    F = hessenberg(A)
-    T = F.Q
-    P = C*T
-    Q = T\B # TODO Type stability? # T is unitary, so mutliplication with T' should do the trick
-    # FIXME; No performance improvement from Hessienberg structure, also weired renaming of matrices
-    StateSpace(F.H, Q, P, D, sys.timeevol)
+    PermutedDimsArray(R, (3,1,2)) # PermutedDimsArray doesn't allocate to perform the permutation
 end
 
 
diff --git a/test/test_delayed_systems.jl b/test/test_delayed_systems.jl
index aae0699e1..cc1d14f95 100644
--- a/test/test_delayed_systems.jl
+++ b/test/test_delayed_systems.jl
@@ -109,7 +109,7 @@ d = exp(-2*s)
 # Test for internal function delayd_ss
 @test freqresp(ControlSystems.delayd_ss(1.0, 0.2), Ω)[:] ≈ exp.(-im*Ω) atol=1e-14
 @test freqresp(ControlSystems.delayd_ss(3.2, 0.4), Ω)[:] ≈ exp.(-3.2*im*Ω) atol=1e-14
-@test_throws ErrorException freqresp(ControlSystems.delayd_ss(3.2, 0.5), Ω)
+@test_throws ErrorException ControlSystems.delayd_ss(3.2, 0.5)
 
 # Simple tests for c2d of DelayLtiSystems
 @test freqresp(c2d(feedback(ss(0,1,1,0), delay(1.5)), 0.5), Ω) ≈ [0.5/((z - 1) + 0.5*z^-3) for z in exp.(im*Ω*0.5)]
diff --git a/test/test_freqresp.jl b/test/test_freqresp.jl
index 1a6c728eb..53384a749 100644
--- a/test/test_freqresp.jl
+++ b/test/test_freqresp.jl
@@ -116,3 +116,34 @@ mag, mag, ws2 = bode(sys2)
 @test minimum(ws2) <= 0.2min(p,z)
 @test length(ws2) > 100
 end
+
+
+## Benchmark code for freqresp
+# sizes = [1:40; 50:5:100; 120:20:300; 800]
+# times1 = map(sizes) do nx
+#     w = exp10.(LinRange(-2, 2, 200))
+#     @show nx
+#     G = ssrand(2,2,nx)
+#     nx == 1 && (freqresp(G, w)) # precompile
+#     GC.gc()
+#     t = @timed freqresp(G, w)
+#     (t.time, t.bytes)
+# end
+# sleep(5)
+# times2 = map(sizes) do nx
+#     w = exp10.(LinRange(-2, 2, 200))
+#     @show nx
+#     G = ssrand(2,2,nx)
+#     # NOTE: rename the freqresp method to be benchmarked before running. Below it's called freqresp_large
+#     nx == 1 && (freqresp_large(G, w)) # precompile
+#     GC.gc()
+#     t = @timed freqresp_large(G, w)
+#     (t.time, t.bytes)
+# end
+
+# f1 = plot(sizes, first.(times1), scale=:log10, lab="Time freqresp", m=:o)
+# plot!(sizes, first.(times2), scale=:log10, lab="Time freqresp_large", xlabel="Model order", m=:o)
+
+# f2 = plot(sizes, last.(times1), scale=:log10, lab="Allocations freqresp", m=:o)
+# plot!(sizes, last.(times2), scale=:log10, lab="Allocations freqresp_large", xlabel="Model order", m=:o)
+# plot(f1, f2)
\ No newline at end of file