fix gain margin crossing at zero freq (#1046)

* fix gain margin crossing at zero freq

* handle edge case

* more filtering

Author: baggepinnen

lib/ControlSystemsBase/src/analysis.jl

@@ -492,7 +492,7 @@ end
 """
     wgm, gm, wpm, pm = margin(sys::LTISystem, w::Vector; full=false, allMargins=false, adjust_phase_start=true)
 
-returns frequencies for gain margins, gain margins, frequencies for phase margins, phase margins
+returns frequencies for gain margins, gain margins (magnitude), frequencies for phase margins, phase margins (degrees)
 
 - If `!allMargins`, return only the smallest margin
 - If `full` return also `fullPhase`
@@ -542,8 +542,19 @@ function sisomargin(sys::LTISystem, w::AbstractVector{<:Real}; full=false, allMa
     mag, phase, w = bode(sys, w)
     wgm, = _allPhaseCrossings(w, phase)
     gm = similar(wgm)
+    remove = Int[]
     for i = eachindex(wgm)
-        gm[i] = 1 ./ abs(freqresp(sys,wgm[i])[1])
+        Giw = freqresp(sys,wgm[i])[1]
+        if sign(w[1]) != sign(w[end]) && abs(Giw) > 1e6 && wgm[i] < 0.001
+            # This tries to filter out extremely large gain margins that can arise when the Nyquist contour crosses the negative real axis at -Inf.
+            # This is filter is in addition to the filter_th check in _findCrossings
+            push!(remove, i)
+        end
+        gm[i] = 1 ./ abs(Giw)
+    end
+    if !isempty(remove)
+        deleteat!(gm, remove)
+        deleteat!(wgm, remove)
     end
     wpm, fi = _allGainCrossings(w, mag)
     pm = similar(wpm)
@@ -578,9 +589,10 @@ function sisomargin(sys::LTISystem, w::AbstractVector{<:Real}; full=false, allMa
     if adjust_phase_start && isrational(sys)
         intexcess, p, z, tol = integrator_excess_with_tol(sys)
         n_unstable_poles = count(real(p) > tol for p in p)
-        if intexcess != 0
+        first_positive_freq_ind = findfirst(>(0), w)
+        if intexcess != 0 && (first_positive_freq_ind !== nothing)
             # Snap phase so that it starts at -90*intexcess
-            nineties = round(Int, phase[1] / 90)
+            nineties = round(Int, phase[first_positive_freq_ind] / 90)
             adjust = ((90*(-intexcess-nineties)) ÷ 360) * 360
             pm_unstable_adjust = n_unstable_poles*360 # count the number of unstable poles, and remove 360 for each. Be careful with poles that are counted as integrators
             pm = pm .+ adjust .- pm_unstable_adjust
@@ -595,9 +607,11 @@ function sisomargin(sys::LTISystem, w::AbstractVector{<:Real}; full=false, allMa
     end
 end
 margin(system::LTISystem; kwargs...) =
-margin(system, _default_freq_vector(system, Val{:bode}()); kwargs...)
+margin(system, _add_eps_negative(_default_freq_vector(system, Val{:bode}())); kwargs...)
 #margin(sys::LTISystem, args...) = margin(LTISystem[sys], args...)
 
+_add_eps_negative(w) = [-(eps(eltype(w))); w] # The size of the negative frequency is a tradeoff, too small and the check `if abs(d) > 20` in _findCrossings may fail, but too large and we get an inaccurate interpolation. 
+
 # Interpolate the values in "list" given the floating point "index" fi
 function interpolate(fi, list)
     fif = floor.(Integer, fi)
@@ -613,19 +627,25 @@ function _allPhaseCrossings(w, phase)
     #Calculate numer of times real axis is crossed on negative side
     n = fld.(phase.+180,360) #Nbr of crossed
     ph = mod.(phase,360) .- 180 #Residual
-    _findCrossings(w, n, ph)
+    _findCrossings(w, n, ph; filter_th=20.0)
 end
 
-function _findCrossings(w, n, res)
+function _findCrossings(w, n, res; filter_th=Inf)
     wcross = Vector{eltype(w)}()
     tcross = Vector{eltype(w)}()
     for i in 1:(length(w)-1)
         if res[i] == 0
             push!(wcross, w[i])
             push!(tcross, i)
         elseif n[i] != n[i+1]
+            d = res[i]-res[i+1]
+            if abs(d) > filter_th && (sign(w[i+1]) != sign(w[i]))
+                # This can happen when there are both negative and positive frequencies and the Nyquist contour crosses the negative real axis at -infinity
+                # This logic is slightly flawed since if there are two frequencies, like -eps, eps that have phase values very close to each other we'll fail to reject the crossing, but to handle this case properly one would have to look at both phase and gain at the same time, which we aren't set-up to do easily at this. We should be able to reject the most common case when the default eps() negative frequency is the only one used
+                continue
+            end
             #Interpolate to approximate crossing
-            t = res[i]/(res[i]-res[i+1])
+            t = res[i]/d
             push!(tcross, i+t)
             wt = w[i]+t*(w[i+1]-w[i])
             push!(wcross, wt)
@@ -649,11 +669,14 @@ The delay margin is computed as the phase margin in radians divided by the cross
 function delaymargin(G::LTISystem)
     # Phase margin in radians divided by cross-over frequency in rad/s.
     issiso(G) || error("delaymargin only supports SISO systems")
-    m     = margin(G,allMargins=true)
-    isempty(m[4][1]) && return Inf
-    ϕₘ, i = findmin(m[4][1])
+    ωgₘ, gₘ, ωϕₘa, ϕₘa = margin(G,allMargins=true)
+    isempty(ϕₘa[1]) && return Inf
+    ϕₘ, i = findmin(sign.(ωϕₘa[1]) .* ϕₘa[1]) # flip sign of negative frequency margins 
     ϕₘ   *= π/180
-    ωϕₘ   = m[3][1][i]
+    ωϕₘ   = ωϕₘa[1][i]
+    if numeric_type(G) <: Real
+        ωϕₘ = abs(ωϕₘ)
+    end
     dₘ    = ϕₘ/ωϕₘ
     if isdiscrete(G)
         dₘ /= G.Ts # Give delay margin in number of sample times, as matlab does

lib/ControlSystemsBase/test/test_analysis.jl

@@ -1,5 +1,6 @@
 @test_throws MethodError poles(big(1.0)*ssrand(1,1,1)) # This errors before loading GenericSchur
 using GenericSchur # Required to compute eigvals (in tzeros and poles) of a matrix with exotic element types
+using Test
 
 es(x) = sort(x, by=LinearAlgebra.eigsortby)
 ## tzeros ##
@@ -209,6 +210,21 @@ G = [1/(s+2) -1/(s+2); 1/(s+2) (s+1)/(s+2)]
 
 ## MARGIN ##
 
+# Test case that requires negative frequencies to be included in the grid in order to find one margin
+# https://github.com/JuliaControl/ControlSystems.jl/issues/1045
+temp = let
+    tempA = [-1.42119805189796 1.0 1.42119805189796 0.0; -1.5105265612217906 -1.4146551592967844 1.009901951359278 0.0; 0.0 0.0 0.0 1.0; 0.0 0.0 0.5 0.0]
+    tempB = [0.0; 0.0; 0.0; 0.1;;]
+    tempC = [10.006246098625127 14.146551592967842 0.0 0.0]
+    tempD = [0.0;;]
+    ss(tempA, tempB, tempC, tempD)
+end
+wgm, gm, wpm, pm = margin(temp, allMargins=true)
+@test wgm[] ≈ [0.0, 1.2311038829891778] atol=1e-3
+@test gm[] ≈ [0.8733647564616344, 2.005125180939021] atol=1e-3
+
+
+
 w = exp10.(LinRange(-1, 2, 100))
 P = tf(1,[1.0, 1])
 ωgₘ, gₘ, ωϕₘ, ϕₘ = margin(P, w)
@@ -231,6 +247,7 @@ marginplot(P, w)
 @test ϕₘ[][] ≥ 50
 @test ωϕₘ[][] ≈ 0.7871132039227572
 
+
 ## Delay margin
 dm = delaymargin(P)[]
 R = freqresp(P*delay(dm), ωϕₘ[][]-0.5:0.0001:ωϕₘ[][]+0.5)