Merge branch 'dev' into simresult

baggepinnen · web-flow · commit 5c0075129ff2 · 2021-10-20T11:46:47.000+02:00
diff --git a/src/analysis.jl b/src/analysis.jl
@@ -89,13 +89,17 @@ function det(sys::Matrix{S}) where {S<:SisoZpk}
 end
 
 """
-    dcgain(sys)
+    dcgain(sys, ϵ=0)
 
 Compute the dcgain of system `sys`.
 
-equal to G(0) for continuous-time systems and G(1) for discrete-time systems."""
-function dcgain(sys::LTISystem)
-    return iscontinuous(sys) ? evalfr(sys, 0) : evalfr(sys, 1)
+equal to G(0) for continuous-time systems and G(1) for discrete-time systems.
+
+`ϵ` can be provided to evaluate the dcgain with a small perturbation into
+the stability region of the complex plane.
+"""
+function dcgain(sys::LTISystem, ϵ=0)
+    return iscontinuous(sys) ? evalfr(sys, -ϵ) : evalfr(sys, exp(-ϵ*sys.Ts))
 end
 
 """
@@ -141,7 +145,7 @@ poles, `ps`, of `sys`"""
 function damp(sys::LTISystem)
     ps = pole(sys)
     if isdiscrete(sys)
-        ps = log.(ps)/sys.Ts
+        ps = log.(complex.(ps))/sys.Ts
     end
     Wn = abs.(ps)
     order = sortperm(Wn; by=z->(abs(z), real(z), imag(z)))
@@ -160,20 +164,29 @@ function dampreport(io::IO, sys::LTISystem)
     Wn, zeta, ps = damp(sys)
     t_const = 1 ./ (Wn.*zeta)
     header =
-    ("|     Pole      |   Damping     |   Frequency   |   Frequency   | Time Constant |\n"*
-     "|               |    Ratio      |   (rad/sec)   |     (Hz)      |     (sec)     |\n"*
-     "+---------------+---------------+---------------+---------------+---------------+")
+    ("|        Pole        |   Damping     |   Frequency   |   Frequency   | Time Constant |\n"*
+     "|                    |    Ratio      |   (rad/sec)   |     (Hz)      |     (sec)     |\n"*
+     "+--------------------+---------------+---------------+---------------+---------------+")
     println(io, header)
     if all(isreal, ps)
         for i=eachindex(ps)
             p, z, w, t = ps[i], zeta[i], Wn[i], t_const[i]
-            Printf.@printf(io, "|  %-13.3e|  %-13.3e|  %-13.3e|  %-13.3e|  %-13.3e|\n", real(p), z, w, w/(2π), t)
+            Printf.@printf(io, "| %-+18.3g |  %-13.3g|  %-13.3g|  %-13.3g|  %-13.3g|\n", real(p), z, w, w/(2π), t)
         end
-    else
+    elseif numeric_type(sys) <: Real # real-coeff system with complex conj. poles
+        for i=eachindex(ps)
+            p, z, w, t = ps[i], zeta[i], Wn[i], t_const[i]
+            imag(p) < 0 && (continue) # use only the positive complex pole to print with the ± operator
+            if imag(p) == 0 # no ± operator for real pole
+                Printf.@printf(io, "| %-+18.3g |  %-13.3g|  %-13.3g|  %-13.3g|  %-13.3g|\n", real(p), z, w, w/(2π), t)
+            else
+                Printf.@printf(io, "| %-+7.3g ± %6.3gim |  %-13.3g|  %-13.3g|  %-13.3g|  %-13.3g|\n", real(p), imag(p), z, w, w/(2π), t)
+            end
+        end
+    else # complex-coeff system
         for i=eachindex(ps)
             p, z, w, t = ps[i], zeta[i], Wn[i], t_const[i]
-            Printf.@printf(io, "|  %-13.3e|  %-13.3e|  %-13.3e|  %-13.3e|  %-13.3e|\n", real(p), z, w, w/(2π), t)
-            Printf.@printf(io, "|  %-+11.3eim|               |               |               |               |\n", imag(p))
+            Printf.@printf(io, "| %-+7.3g  %+7.3gim |  %-13.3g|  %-13.3g|  %-13.3g|  %-13.3g|\n", real(p), imag(p), z, w, w/(2π), t)
         end
     end
 end
@@ -521,7 +534,7 @@ function gangofseven(P::TransferFunction, C::TransferFunction, F::TransferFuncti
     end
     S, PS, CS, T = gangoffour(P,C)
     RY = T*F
-    RU = N*F
+    RU = CS*F
     RE = S*F
     return S, PS, CS, T, RY, RU, RE
 end
diff --git a/src/connections.jl b/src/connections.jl
@@ -347,7 +347,7 @@ feedback2dof(B,A,R,S,T) = tf(conv(B,T),zpconv(A,R,B,S))
 Return the transfer function
 `P(F+C)/(1+PC)`
 which is the closed-loop system with process `P`, controller `C` and feedforward filter `F` from reference to control signal (by-passing `C`).
-
+```
          +-------+
          |       |
    +----->   F   +----+
@@ -360,6 +360,7 @@ r  |  -  |       |    |    |       |    y
       |  +-------+         +-------+   |
       |                                |
       +--------------------------------+
+```
 """
 function feedback2dof(P::TransferFunction{TE}, C::TransferFunction{TE}, F::TransferFunction{TE}) where TE
     !issiso(P) && error("Feedback not implemented for MIMO systems")
diff --git a/src/delay_systems.jl b/src/delay_systems.jl
@@ -180,7 +180,7 @@ function _lsim(sys::DelayLtiSystem{T,S}, Base.@nospecialize(u!), t::AbstractArra
     p = (A, B1, B2, C1, C2, D11, D12, D21, D22, y, Tau, u!, uout, hout, tmpy, t)
 
     # This callback computes and stores the delay term
-    sv = SavedValues(Float64, Tuple{Vector{Float64},Vector{Float64}})
+    sv = SavedValues(eltype(t), Tuple{Vector{T},Vector{T}})
     cb = SavingCallback(dde_saver, sv, saveat = t)
 
     # History function, only used for d
diff --git a/src/freqresp.jl b/src/freqresp.jl
@@ -57,7 +57,7 @@ function evalfr(sys::AbstractStateSpace, s::Number)
         R = s*I - sys.A
         sys.D + sys.C*((R\sys.B))
     catch e
-        @warn "Got exception $e, returning Inf" max_log=1
+        @warn "Got exception $e, returning Inf" maxlog=1
         fill(convert(T, Inf), size(sys))
     end
 end
diff --git a/src/matrix_comps.jl b/src/matrix_comps.jl
@@ -168,8 +168,9 @@ function covar(sys::AbstractStateSpace, W)
     func = iscontinuous(sys) ? lyap : dlyap
     Q = try
         func(A, B*W*B')
-    catch
-        error("No solution to the Lyapunov equation was found in covar")
+    catch e
+        @error("No solution to the Lyapunov equation was found in covar.")
+        rethrow(e)
     end
     P = C*Q*C'
     if !isdiscrete(sys)
@@ -493,9 +494,9 @@ end
 
 
 """
-`sysr, G = balreal(sys::StateSpace)`
+`sysr, G, T = balreal(sys::StateSpace)`
 
-Calculates a balanced realization of the system sys, such that the observability and reachability gramians of the balanced system are equal and diagonal `G`
+Calculates a balanced realization of the system sys, such that the observability and reachability gramians of the balanced system are equal and diagonal `G`. `T` is the similarity transform between the old state `x` and the new state `z` such that `Tz = x`.
 
 See also `gram`, `baltrunc`
 
@@ -531,23 +532,25 @@ function balreal(sys::ST) where ST <: AbstractStateSpace
         display(Σ)
     end
 
-    sysr = ST(T*sys.A/T, T*sys.B, sys.C/T, sys.D, sys.timeevol), diagm(0 => Σ)
+    sysr = ST(T*sys.A/T, T*sys.B, sys.C/T, sys.D, sys.timeevol), diagm(Σ), T
 end
 
 
 """
-    sysr, G = baltrunc(sys::StateSpace; atol = √ϵ, rtol=1e-3, unitgain=true, n = nothing)
+    sysr, G, T = baltrunc(sys::StateSpace; atol = √ϵ, rtol=1e-3, unitgain=true, n = nothing)
 
 Reduces the state dimension by calculating a balanced realization of the system sys, such that the observability and reachability gramians of the balanced system are equal and diagonal `G`, and truncating it to order `n`. If `n` is not provided, it's chosen such that all states corresponding to singular values less than `atol` and less that `rtol σmax` are removed.
 
+`T` is the similarity transform between the old state `x` and the newstate `z` such that `Tz = x`.
+
 If `unitgain=true`, the matrix `D` is chosen such that unit static gain is achieved.
 
 See also `gram`, `balreal`
 
 Glad, Ljung, Reglerteori: Flervariabla och Olinjära metoder
 """
 function baltrunc(sys::ST; atol = sqrt(eps()), rtol = 1e-3, unitgain = true, n = nothing) where ST <: AbstractStateSpace
-    sysbal, S = balreal(sys)
+    sysbal, S, T = balreal(sys)
     S = diag(S)
     if n === nothing
         S = S[S .>= atol]
@@ -564,7 +567,7 @@ function baltrunc(sys::ST; atol = sqrt(eps()), rtol = 1e-3, unitgain = true, n =
         D = D/(C*inv(-A)*B)
     end
 
-    return ST(A,B,C,D,sys.timeevol), diagm(0 => S)
+    return ST(A,B,C,D,sys.timeevol), diagm(S), T
 end
 
 """
diff --git a/src/plotting.jl b/src/plotting.jl
@@ -538,21 +538,24 @@ end
 
 @userplot Sigmaplot
 """
-    sigmaplot(sys, args...)
-    sigmaplot(LTISystem[sys1, sys2...], args...)
+    sigmaplot(sys, args...; hz=false)
+    sigmaplot(LTISystem[sys1, sys2...], args...; hz=false)
 
 Plot the singular values of the frequency response of the `LTISystem`(s). A
 frequency vector `w` can be optionally provided.
 
+If `hz=true`, the plot x-axis will be displayed in Hertz, the input frequency vector is still treated as rad/s.
+
 `kwargs` is sent as argument to Plots.plot.
 """
 sigmaplot
-@recipe function sigmaplot(p::Sigmaplot)
+@recipe function sigmaplot(p::Sigmaplot; hz=false)
     systems, w = _processfreqplot(Val{:sigma}(), p.args...)
+    ws = (hz ? 1/(2π) : 1) .* w
     ny, nu = size(systems[1])
     nw = length(w)
     title --> "Sigma Plot"
-    xguide --> "Frequency (rad/s)",
+    xguide --> (hz ? "Frequency [Hz]" : "Frequency [rad/s]")
     yguide --> "Singular Values $_PlotScaleStr"
     for (si, s) in enumerate(systems)
         sv = sigma(s, w)[1]
@@ -564,7 +567,7 @@ sigmaplot
                 xscale --> :log10
                 yscale --> _PlotScaleFunc
                 seriescolor --> si
-                w, sv[:, i]
+                ws, sv[:, i]
             end
         end
     end
@@ -666,9 +669,6 @@ Create a pole-zero map of the `LTISystem`(s) in figure `fig`, `args` and `kwargs
 pzmap
 @recipe function pzmap(p::Pzmap)
     systems = p.args[1]
-    if systems[1].nu + systems[1].ny > 2
-        @warn("pzmap currently only supports SISO systems. Only transfer function from u₁ to y₁ will be shown")
-    end
     seriestype := :scatter
     framestyle --> :zerolines
     title --> "Pole-zero map"
diff --git a/src/simplification.jl b/src/simplification.jl
@@ -4,14 +4,24 @@
 Compute the structurally minimal realization of the state-space system `sys`. A
 structurally minimal realization is one where only states that can be
 determined to be uncontrollable and unobservable based on the location of 0s in
-`sys` are removed."""
+`sys` are removed.
+
+Systems with numerical noise in the coefficients, e.g., noise on the order of `eps` require truncation to zero to be
+affected by structural simplification, e.g.,
+```julia
+trunc_zero!(A) = A[abs.(A) .< 10eps(maximum(abs, A))] .= 0
+trunc_zero!(sys.A); trunc_zero!(sys.B); trunc_zero!(sys.C)
+sminreal(sys)
+```
+"""
 function sminreal(sys::StateSpace)
     A, B, C, inds = struct_ctrb_obsv(sys)
     return StateSpace(A, B, C, sys.D, sys.timeevol)
 end
 
 # Determine the structurally controllable and observable realization for the system
 struct_ctrb_obsv(sys::StateSpace) = struct_ctrb_obsv(sys.A, sys.B, sys.C)
+
 function struct_ctrb_obsv(A::AbstractVecOrMat, B::AbstractVecOrMat, C::AbstractVecOrMat)
     costates = struct_ctrb_states(A, B) .& struct_ctrb_states(A', C')
     if !all(costates)
@@ -22,15 +32,17 @@ function struct_ctrb_obsv(A::AbstractVecOrMat, B::AbstractVecOrMat, C::AbstractV
     end
 end
 
-# Compute a bit-vector, expressing whether a state of the pair (A, B) is
-# structurally controllable.
+"""
+    struct_ctrb_states(A::AbstractVecOrMat, B::AbstractVecOrMat)
+
+Compute a bit-vector, expressing whether a state of the pair (A, B) is
+structurally controllable based on the location of zeros in the matrices. 
+"""
 function struct_ctrb_states(A::AbstractVecOrMat, B::AbstractVecOrMat)
-    bitA = A .!= 0
-    d_cvec = cvec = vec(any(B .!= 0, dims=2))
-    while any(d_cvec .!= 0)
-        Adcvec = vec(any(bitA[:, findall(d_cvec)], dims=2))
-        cvec = cvec .| Adcvec
-        d_cvec = Adcvec .& .~cvec
+    bitA = .!iszero.(A)
+    x = vec(any(B .!= 0, dims=2)) # index vector indicating states that have been affected by input
+    for i = 1:size(A, 1) # apply A nx times, similar to controllability matrix
+        x = x .| .!iszero.(bitA * x)
     end
-    return cvec
+    x
 end
diff --git a/src/timeresp.jl b/src/timeresp.jl
@@ -17,7 +17,7 @@ function Base.step(sys::AbstractStateSpace, t::AbstractVector; method=:cont, kwa
     nx = nstates(sys)
     u_element = [one(eltype(t))] # to avoid allocating this multiple times
     u = (x,t)->u_element
-    x0 = zeros(nx)
+    x0 = zeros(T, nx)
     if nu == 1
         y, tout, x, uout = lsim(sys, u, t; x0, method, kwargs...)
     else
diff --git a/src/types/DelayLtiSystem.jl b/src/types/DelayLtiSystem.jl
@@ -160,7 +160,7 @@ function Base.show(io::IO, sys::DelayLtiSystem)
     print(io, "\nP: ")
     show(io, sys.P.P)
 
-    println(io, "\n\nDelays: $(sys.Tau)")
+    print(io, "\n\nDelays: $(sys.Tau)")
 end
 
 
diff --git a/src/types/StateSpace.jl b/src/types/StateSpace.jl
@@ -217,6 +217,8 @@ function isapprox(sys1::ST1, sys2::ST2; kwargs...) where {ST1<:AbstractStateSpac
 end
 
 ## ADDITION ##
+Base.zero(sys::AbstractStateSpace) = ss(zero(sys.D), sys.timeevol)
+
 function +(s1::StateSpace{TE,T}, s2::StateSpace{TE,T}) where {TE,T}
     #Ensure systems have same dimensions
     if size(s1) != size(s2)
diff --git a/src/types/TransferFunction.jl b/src/types/TransferFunction.jl
@@ -33,6 +33,8 @@ ninputs(G::TransferFunction) = size(G.matrix, 2)
 Base.ndims(::TransferFunction) = 2
 Base.size(G::TransferFunction) = size(G.matrix)
 Base.eltype(::Type{S}) where {S<:TransferFunction} = S
+Base.zero(G::TransferFunction{TE,S}) where {TE,S} = tf(zeros(numeric_type(S), size(G)), G.timeevol) # can not create a zero of a discrete system from the type alone, the sampletime is not stored.
+
 
 function Base.getindex(G::TransferFunction{TE,S}, inds...) where {TE,S<:SisoTf}
     if size(inds, 1) != 2
diff --git a/src/types/conversion.jl b/src/types/conversion.jl
@@ -173,7 +173,7 @@ function balance_statespace(A::AbstractMatrix, B::AbstractMatrix, C::AbstractMat
         @warn "Unable to balance state-space, returning original system"
         return A,B,C,I
     end
- end
+end
 
 # # First try to promote and hopefully get some types we can work with
 # function balance_statespace(A::AbstractMatrix, B::AbstractMatrix, C::AbstractMatrix, perm::Bool=false)
diff --git a/src/utilities.jl b/src/utilities.jl
@@ -32,26 +32,12 @@ to_abstract_matrix(A::AbstractVector) = reshape(A, length(A), 1)
 to_abstract_matrix(A::Number) = fill(A, 1, 1)
 
 # Do no sorting of eigenvalues
-@static if VERSION > v"1.2.0-DEV.0"
-    eigvalsnosort(args...; kwargs...) = eigvals(args...; sortby=nothing, kwargs...)
-    roots(args...; kwargs...) = Polynomials.roots(args...; sortby=nothing, kwargs...)
-else
-    eigvalsnosort(args...; kwargs...) = eigvals(args...; kwargs...)
-    roots(args...; kwargs...) = Polynomials.roots(args...; kwargs...)
-end
+eigvalsnosort(args...; kwargs...) = eigvals(args...; sortby=nothing, kwargs...)
+roots(args...; kwargs...) = Polynomials.roots(args...; sortby=nothing, kwargs...)
 
 issemiposdef(A) = ishermitian(A) && minimum(real.(eigvals(A))) >= 0
 issemiposdef(A::UniformScaling) = real(A.λ) >= 0
 
-@static if VERSION < v"1.1.0-DEV"
-    #Added in 1.1.0-DEV
-    LinearAlgebra.isposdef(A::UniformScaling) = isposdef(A.λ)
-end
-@static if VERSION < v"1.1"
-    isnothing(::Any) = false
-    isnothing(::Nothing) = true
-end
-
 """ f = printpolyfun(var)
 `fun` Prints polynomial in descending order, with variable `var`
 """
diff --git a/test/framework.jl b/test/framework.jl
@@ -1,3 +1,11 @@
+# Set plot globals
+ENV["PLOTS_TEST"] = "true"
+ENV["GKSwstype"] = "nul"
+
+using Plots
+gr()
+default(show=false)
+
 using ControlSystems
 # Local definition to make sure we get warnings if we use eye
 eye_(n) = Matrix{Int64}(I, n, n)
diff --git a/test/test_analysis.jl b/test/test_analysis.jl
@@ -96,6 +96,7 @@ ex_11 = ss(A, B, C, D)
 @test tzero(ex_11) ≈ [4.0, -3.0]
 
 # Test for multiple zeros, siso tf
+s = tf("s")
 sys = s*(s + 1)*(s^2 + 1)*(s - 3)/((s + 1)*(s + 4)*(s - 4))
 @test tzero(sys) ≈ [3.0, -1.0, im, -im, 0.0]
 
@@ -149,6 +150,8 @@ z, p, k = zpkdata(G)
 ## GAIN ## #Gain is confusing when referring to zpkdata. Test dcgain instead
 @test [dcgain(H[1, 1]) dcgain(H[1, 2]); dcgain(H[2, 1]) dcgain(H[2, 2])] ≈ [0 0; 0.2 1/3]
 @test [dcgain(G[1, 1]) dcgain(G[1, 2]); dcgain(G[2, 1]) dcgain(G[2, 2])] ≈ [0 0; 0.2 1/3]
+@test dcgain(H[1, 1], 1e-6)[] == 0
+@test dcgain(H[2, 1], 1e-6)[] ≈ 0.2 rtol=1e-5
 
 ## MARKOVPARAM ##
 @test markovparam(G, 0) == [0.0 0.0; 1.0 0.0]
@@ -172,11 +175,13 @@ damp_output = damp(ex_11)
 
 
 ## DAMPREPORT ##
-@test sprint(dampreport, sys) == (
-     "|     Pole      |   Damping     |   Frequency   |   Frequency   | Time Constant |\n|               |    Ratio      |   (rad/sec)   |     (Hz)      |     (sec)     |\n+---------------+---------------+---------------+---------------+---------------+\n|  -1.000e+00   |  1.000e+00    |  1.000e+00    |  1.592e-01    |  1.000e+00    |\n|  4.000e+00    |  -1.000e+00   |  4.000e+00    |  6.366e-01    |  -2.500e-01   |\n|  -4.000e+00   |  1.000e+00    |  4.000e+00    |  6.366e-01    |  2.500e-01    |\n")
+s = tf("s")
+sys = s*(s + 1)*(s^2 + 1)*(s - 3)/((s + 1)*(s + 4)*(s - 4))
+@test sprint(dampreport, sys) == "|        Pole        |   Damping     |   Frequency   |   Frequency   | Time Constant |\n|                    |    Ratio      |   (rad/sec)   |     (Hz)      |     (sec)     |\n+--------------------+---------------+---------------+---------------+---------------+\n| -1                 |  1            |  1            |  0.159        |  1            |\n| +4                 |  -1           |  4            |  0.637        |  -0.25        |\n| -4                 |  1            |  4            |  0.637        |  0.25         |\n"
+
+@test sprint(dampreport, ex_4) == "|        Pole        |   Damping     |   Frequency   |   Frequency   | Time Constant |\n|                    |    Ratio      |   (rad/sec)   |     (Hz)      |     (sec)     |\n+--------------------+---------------+---------------+---------------+---------------+\n| +0                 |  -1           |  0            |  0            |  -Inf         |\n| -0.0597 ± 0.0171im |  0.961        |  0.0621       |  0.00989      |  16.7         |\n| -0.0858            |  1            |  0.0858       |  0.0137       |  11.7         |\n| -0.18              |  1            |  0.18         |  0.0287       |  5.55         |\n"
 
-@test sprint(dampreport, 1/(s+1+2im)/(s+2+3im)) == (
-     "|     Pole      |   Damping     |   Frequency   |   Frequency   | Time Constant |\n|               |    Ratio      |   (rad/sec)   |     (Hz)      |     (sec)     |\n+---------------+---------------+---------------+---------------+---------------+\n|  -1.000e+00   |  4.472e-01    |  2.236e+00    |  3.559e-01    |  1.000e+00    |\n|  -2.000e+00 im|               |               |               |               |\n|  -2.000e+00   |  5.547e-01    |  3.606e+00    |  5.738e-01    |  5.000e-01    |\n|  -3.000e+00 im|               |               |               |               |\n")
+@test sprint(dampreport, 1/(s+1+2im)/(s+2+3im)) == "|        Pole        |   Damping     |   Frequency   |   Frequency   | Time Constant |\n|                    |    Ratio      |   (rad/sec)   |     (Hz)      |     (sec)     |\n+--------------------+---------------+---------------+---------------+---------------+\n| -1            -2im |  0.447        |  2.24         |  0.356        |  1            |\n| -2            -3im |  0.555        |  3.61         |  0.574        |  0.5          |\n"
 
 # Example 5.5 from http://www.control.lth.se/media/Education/EngineeringProgram/FRTN10/2017/e05_both.pdf
 G = [1/(s+2) -1/(s+2); 1/(s+2) (s+1)/(s+2)]
diff --git a/test/test_connections.jl b/test/test_connections.jl
diff --git a/test/test_delayed_systems.jl b/test/test_delayed_systems.jl
diff --git a/test/test_plots.jl b/test/test_plots.jl
diff --git a/test/test_simplification.jl b/test/test_simplification.jl
diff --git a/test/test_statespace.jl b/test/test_statespace.jl
