add in-place version of freqresp (#644)

* add in-place version of `freqresp`

* inplace freqresp for delay systems

Author: baggepinnen

src/ControlSystems.jl

@@ -72,7 +72,7 @@ export  LTISystem,
         solve,
         Simulator,
         # Frequency Response
-        freqresp, freqrespv,
+        freqresp, freqrespv, freqresp!,
         evalfr,
         bode, bodev,
         nyquist, nyquistv,

src/delay_systems.jl

@@ -1,11 +1,9 @@
-function freqresp(sys::DelayLtiSystem, ω::AbstractVector{T}) where {T <: Real}
+function freqresp!(R::Array{T,3}, sys::DelayLtiSystem, ω::AbstractVector{W}) where {T, W <: Real}
     ny = noutputs(sys)
     nu = ninputs(sys)
-
+    @boundscheck size(R) == (ny,nu,length(ω))
     P_fr = freqresp(sys.P.P, ω).parent
 
-    G_fr = zeros(eltype(P_fr), ny, nu, length(ω))
-
     cache = cis.(ω[1].*sys.Tau)
 
     @views for ω_idx=1:length(ω)
@@ -17,10 +15,9 @@ function freqresp(sys::DelayLtiSystem, ω::AbstractVector{T}) where {T <: Real}
         delay_matrix_inv_fr = Diagonal(cache) # Frequency response of the diagonal matrix with delays
         # Inverse of the delay matrix, so there should not be any minus signs in the exponents
 
-        G_fr[:,:,ω_idx] .= P11_fr .+ P12_fr/(delay_matrix_inv_fr - P22_fr)*P21_fr # The matrix is invertible (?!)
+        R[:,:,ω_idx] .= P11_fr .+ P12_fr/(delay_matrix_inv_fr - P22_fr)*P21_fr # The matrix is invertible (?!)
     end
-
-    return PermutedDimsArray(G_fr, (3,1,2))
+    return PermutedDimsArray{T,3,(3,1,2),(2,3,1),Array{T,3}}(R)
 end
 
 function evalfr(sys::DelayLtiSystem, s)

src/freqresp.jl

@@ -19,10 +19,27 @@ Evaluate the frequency response of a linear system
 
 of system `sys` over the frequency vector `w`.
 """
-@autovec () function freqresp(sys::LTISystem, w_vec::AbstractVector{<:Real})
+@autovec () function freqresp(sys::LTISystem, w_vec::AbstractVector{W}) where W <: Real
+    te = timeevol(sys)
+    ny,nu = noutputs(sys), ninputs(sys)
+    T = promote_type(Complex{real(numeric_type(sys))}, Complex{W})
+    R = Array{T, 3}(undef, ny, nu, length(w_vec))
+    freqresp!(R, sys, w_vec)
+end
+
+"""
+    freqresp!(R::Array{T, 3}, sys::LTISystem, w_vec::AbstractVector{<:Real})
+
+In-place version of [`freqresp`](@ref) that takes a pre-allocated array `R` of size (ny, nu, nw)`
+"""
+function freqresp!(R::Array{T,3}, sys::LTISystem, w_vec::AbstractVector{<:Real}) where T
     te = sys.timeevol
     ny,nu = noutputs(sys), ninputs(sys)
-    [evalfr(sys[i,j], _freq(w, te))[] for w in w_vec, i in 1:ny, j in 1:nu]
+    @boundscheck size(R) == (ny,nu,length(w_vec))
+    @inbounds for wi = eachindex(w_vec), ui = 1:nu, yi = 1:ny
+        R[yi,ui,wi] = evalfr(sys[yi,ui], _freq(w_vec[wi], te))[]
+    end
+    PermutedDimsArray{T,3,(3,1,2),(2,3,1),Array{T,3}}(R)
 end
 
 @autovec () function freqresp(G::AbstractMatrix, w_vec::AbstractVector{<:Real})
@@ -36,20 +53,29 @@ end
 _freq(w, ::Continuous) = complex(0, w)
 _freq(w, te::Discrete) = cis(w*te.Ts)
 
-@autovec () function freqresp(sys::AbstractStateSpace, w_vec::AbstractVector{W}) where W <: Real
+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})
+    R = Array{T, 3}(undef, ny, nu, length(w_vec))
+    freqresp!(R, sys, w_vec)
+end
+@autovec () function freqresp!(R::Array{T,3}, sys::AbstractStateSpace, w_vec::AbstractVector{W}) where {T, W <: Real}
+    ny, nu = size(sys)
+    @boundscheck size(R) == (ny,nu,length(w_vec))
     PDT = PermutedDimsArray{T,3,(3,1,2),(2,3,1),Array{T,3}}
     if sys.nx == 0 # Only D-matrix
-        return PDT(repeat(T.(sys.D), 1, 1, length(w_vec)))
+        @inbounds for i in eachindex(w_vec)
+            R[:,:,i] .= sys.D
+        end
+        return PDT(R)
     end
     local F, Q
     try
         F = hessenberg(sys.A)
         Q = Matrix(F.Q)
     catch e
         # For matrix types that do not have a hessenberg implementation, we call the standard version of freqresp.
-        e isa MethodError && return freqresp_nohess(sys, w_vec)
+        e isa MethodError && return freqresp_nohess!(R, sys, w_vec)
         rethrow()
     end
     A = F.H
@@ -58,9 +84,8 @@ _freq(w, te::Discrete) = cis(w*te.Ts)
     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)
+    @inbounds for i in eachindex(w_vec)
         Ri = @views R[:,:,i]
         copyto!(Ri,D) # start with the D-matrix
         isinf(w_vec[i]) && continue
@@ -72,27 +97,36 @@ _freq(w, te::Discrete) = cis(w*te.Ts)
     PDT(R) # PermutedDimsArray doesn't allocate to perform the permutation
 end
 
+function freqresp_nohess(sys::AbstractStateSpace, w_vec::AbstractVector{W}) where W <: Real
+    ny, nu = size(sys)
+    T = promote_type(Complex{real(eltype(sys.A))}, Complex{W})
+    R = Array{T, 3}(undef, ny, nu, length(w_vec))
+    freqresp_nohess!(R, sys, w_vec)
+end
+
 """
     freqresp_nohess(sys::AbstractStateSpace, w_vec::AbstractVector{<:Real})
 
 Compute the frequency response of `sys` without forming a Hessenberg factorization.
 This function is called automatically if the Hessenberg factorization fails.
 """
 freqresp_nohess
-@autovec () function freqresp_nohess(sys::AbstractStateSpace, w_vec::AbstractVector{W}) where W <: Real
+@autovec () function freqresp_nohess!(R::Array{T,3}, sys::AbstractStateSpace, w_vec::AbstractVector{W}) where {T, W <: Real}
     ny, nu = size(sys)
+    @boundscheck size(R) == (ny,nu,length(w_vec))
     nx = sys.nx
-    T = promote_type(Complex{real(eltype(sys.A))}, Complex{W})
     PDT = PermutedDimsArray{T,3,(3,1,2),(2,3,1),Array{T,3}}
     if nx == 0 # Only D-matrix
-        return PDT(repeat(T.(sys.D), 1, 1, length(w_vec)))
+        @inbounds for i in eachindex(w_vec)
+            R[:,:,i] .= sys.D
+        end
+        return PDT(R)
     end
     A,B,C,D = ssdata(sys)
     te = sys.timeevol
-    R = Array{T, 3}(undef, ny, nu, length(w_vec))
     Ac = (A+one(T)*I) # for storage
     Adiag = diagind(A)
-    for i in eachindex(w_vec)
+    @inbounds for i in eachindex(w_vec)
         Ri = @views R[:,:,i]
         copyto!(Ri,D) # start with the D-matrix
         isinf(w_vec[i]) && continue