Compute phase margins independently of the unwrapped phase

lib/ControlSystemsBase/src/analysis.jl

@@ -523,13 +523,14 @@ end
 
 returns frequencies for gain margins, gain margins (magnitude), frequencies for phase margins, phase margins (degrees)
 
-- If `!allMargins`, return only the smallest margin
+- Phase margins are signed angles in `[-180°, 180°)`, independent of the unwrapped Bode phase.
+- If `!allMargins`, return the smallest gain margin and the phase margin closest to zero
 - If `full` return also `fullPhase`
 - If `w` is omitted, a gain-aware default frequency range is computed for each
   input-output channel separately, so that `margin(sys)` and `margin(sys[i, j])`
   agree for channel `(i, j)`. An explicit frequency vector is used for all
   channels and limits margin detection to that range.
-- `adjust_phase_start`: If true, the phase will be adjusted so that it starts at -90*intexcess degrees, where `intexcess` is the integrator excess of the system.
+- `adjust_phase_start`: If true, adjust the unwrapped phase returned with `full=true` and used for plotting so that it starts at -90*intexcess degrees, where `intexcess` is the integrator excess. This does not affect the margins.
 - `balance`: Call [`balance_statespace`](@ref) on each SISO channel before computing the frequency response, see [`freqresp`](@ref).
 
 See also [`delaymargin`](@ref) and [`RobustAndOptimalControl.diskmargin`](https://juliacontrol.github.io/RobustAndOptimalControl.jl/dev/api/#RobustAndOptimalControl.diskmargin)
@@ -576,7 +577,7 @@ _sminreal(sys::LTISystem) = sys
 """
     ωgm, gm, ωpm, pm = sisomargin(sys::LTISystem, w::Vector; full=false, allMargins=false, adjust_phase_start=true, balance=true))
 
-Return frequencies for gain margins, gain margins, frequencies for phase margins, phase margins. If `allMargins=false`, only the smallest margins are returned.
+Return gain-margin frequencies, gain margins, phase-margin frequencies, and signed phase margins in `[-180°, 180°)`. If `allMargins=false`, return the smallest gain margin and the phase margin closest to zero.
 
 `balance`: Call [`balance_statespace`](@ref) on the system before computing the frequency response, see [`freqresp`](@ref).
 """
@@ -612,18 +613,15 @@ function sisomargin(sys::LTISystem, w::AbstractVector{<:Real}; full=false, allMa
     wpm, fi = _allGainCrossings(w, mag)
     pm = similar(wpm)
     for i = eachindex(wpm)
-        # We have to access the actual phase value from the `phase` array to get unwrapped phase. This value is not fully accurate since it is computed at a grid point, so we compute the more accurate phase at the interpolated frequency. This accurate value is not unwrapped, so we add an integer multiple of 360 to get the closest unwrapped phase.
-        φ_nom = rad2deg(angle(freqresp(sys, wpm[i]; balance=false)[1]))
-        φ_rounded = phase[clamp(round(Int, fi[i]), 1, length(phase))] # fi is interpolated, so we round to the closest integer
-        φ_int = φ_nom - 360 * round( (φ_nom - φ_rounded) / 360 )
-
-        # Now compute phase margin relative to -180:
-        pm[i] = 180 + φ_int
+        # Measure the signed angle to the nearest negative-real-axis crossing.
+        φ = rad2deg(angle(freqresp(sys, wpm[i]; balance=false)[1]))
+        pm[i] = mod(φ, 360) - 180
     end
     if !allMargins #Only output the smallest margins
         gm, idx = findmin([gm;Inf])
         wgm = [wgm;NaN][idx]
-        pm, idx = findmin([pm;Inf])
+        _, idx = findmin(abs.([pm;Inf]))
+        pm = [pm;Inf][idx]
         fi = [fi;NaN][idx]
         wpm = [wpm;NaN][idx]
         if full
@@ -639,18 +637,13 @@ function sisomargin(sys::LTISystem, w::AbstractVector{<:Real}; full=false, allMa
             fullPhase = interpolate(fi, phase)
         end
     end
-    if adjust_phase_start && isrational(sys)
-        intexcess, p, z, tol = integrator_excess_with_tol(sys)
-        n_unstable_poles =
-            iscontinuous(sys) ? count(real(pi) > tol for pi in p) :
-            count(abs(pi) > 1 + tol for pi in p)
+    if full && adjust_phase_start && isrational(sys)
+        intexcess = integrator_excess(sys)
         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[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
             phase .+= adjust
             fullPhase = fullPhase .+ adjust
         end
@@ -716,7 +709,7 @@ end
 Return the delay margin, dₘ. For discrete-time systems, the delay margin is normalized by the sample time, i.e., the value represents the margin in number of sample times. 
 Only supports SISO systems.
 
-The delay margin is computed as the phase margin in radians divided by the cross-over frequency in rad/s. The delay margin is the maximum time delay that can be added to the system before it becomes unstable.
+For a nominally stable closed loop, the delay margin is the maximum additional time delay before loss of stability. At each gain crossover, the nonnegative phase lag to the negative real axis is divided by the absolute crossover frequency. The smallest of these delays is returned. Closed-loop stability without delay is assumed, not checked.
 
 `balance`: Call [`balance_statespace`](@ref) on the system before computing the frequency response, see [`freqresp`](@ref).
 """
@@ -725,13 +718,14 @@ function delaymargin(G::LTISystem; balance=true)
     issiso(G) || error("delaymargin only supports SISO systems")
     ωgₘ, gₘ, ωϕₘa, ϕₘa = margin(G; allMargins=true, balance)
     isempty(ϕₘa[1]) && return Inf
-    ϕₘ, i = findmin(sign.(ωϕₘa[1]) .* ϕₘa[1]) # flip sign of negative frequency margins 
-    ϕₘ   *= π/180
-    ωϕₘ   = ωϕₘa[1][i]
-    if numeric_type(G) <: Real
-        ωϕₘ = abs(ωϕₘ)
+    dₘ = oftype(float(real(zero(numeric_type(G)))), Inf)
+    for (ω, ϕ) in zip(ωϕₘa[1], ϕₘa[1])
+        ω, ϕ = real(ω), real(ϕ)
+        # A delay rotates positive and negative frequencies in opposite directions.
+        lag = mod(ω < 0 ? -ϕ : ϕ, 360)
+        candidate = iszero(ω) ? (iszero(lag) ? zero(dₘ) : oftype(dₘ, Inf)) : deg2rad(lag)/abs(ω)
+        dₘ = min(dₘ, candidate)
     end
-    dₘ    = ϕₘ/ωϕₘ
     if isdiscrete(G)
         dₘ /= G.Ts # Give delay margin in number of sample times, as matlab does
     end

lib/ControlSystemsBase/test/runtests.jl

@@ -38,6 +38,7 @@ my_tests = [
             "test_freqresp",
             "test_timeresp",
             "test_analysis",
+            "test_margin_phase",
             "test_matrix_comps",
             "test_synthesis",
             "test_pid_design",

lib/ControlSystemsBase/test/test_analysis.jl

@@ -491,7 +491,7 @@ OL = (ss(Cb)*ss(C1)*ss(C2)*ss(C3)*exp(-3*tau*s))/((C1 - a*s)*(C2 - a*s)*(C3 - a*
 @test ControlSystemsBase._default_freq_vector(OL, Val(:margin)) ==
       ControlSystemsBase._default_freq_vector(OL, Val(:bode))
 wgm, gm, ωϕₘ, ϕₘ = margin(OL; full=true, allMargins=true)
-@test ϕₘ[][] ≈ -320 rtol=1e-2
+@test ϕₘ[][] ≈ 40 atol=0.1
 for wgm in wgm[]
      @test mod(rad2deg(angle(freqresp(OL, wgm)[])), 360)-180 ≈ 0 atol=1e-1
 end

lib/ControlSystemsBase/test/test_margin_phase.jl

@@ -0,0 +1,67 @@
+@testset "Phase margin branches" begin
+    s = tf("s")
+    for ε in (0.0, 1e-6, 1e-4), representation in (tf, zpk, ss)
+        L = representation(2.7(s+1)^2/((s-ε)^2*(s+ε)))
+        @test isstable(feedback(L))
+        ε == 0 || @test ControlSystemsBase.count_integrators(L) == 0
+        for adjust in (false, true)
+            m = margin(L; full=true, adjust_phase_start=adjust)
+            ω = m.wpm[]
+            expected = rad2deg(2atan(ω) - π/2 - atan(ε/ω))
+            @test m.pm[] ≈ expected atol=1e-7
+            @test margin(L; adjust_phase_start=adjust).pm[] ≈ m.pm[]
+            all = margin(L; allMargins=true, adjust_phase_start=adjust)
+            @test only(all.pm[]) ≈ m.pm[]
+            @test only(all.wpm[]) ≈ ω
+            @test rem(m.fullPhase[] - m.pm[] + 180, 360, RoundNearest) ≈ 0 atol=0.02
+        end
+        τ = delaymargin(L)
+        @test τ ≈ (2atan(3) - π/2 - atan(ε/3))/3 rtol=5e-4
+        @test isstable(feedback(L*pade(0.95τ, 6)))
+        @test !isstable(feedback(L*pade(1.05τ, 6)))
+    end
+
+    # Starting the grid above the displaced poles selects a different Bode phase branch.
+    L = ss(2.7(s+1)^2/((s-1e-4)^2*(s+1e-4)))
+    local_grid = [2.0, 3.0, 4.0]
+    wide_grid = [0.0; exp10.(range(-7, 0; length=100)); local_grid]
+    @test margin(L, local_grid).pm[] ≈ margin(L, wide_grid).pm[]
+
+    # Excess phase lag must produce a negative signed margin.
+    L = 27/(s+1)^3
+    m = margin(L, [1.0, sqrt(8), 4.0])
+    @test m.pm[] ≈ 180 - 3rad2deg(atan(sqrt(8))) atol=0.02
+    @test m.pm[] < 0
+
+    # Delay contributes phase revolutions without changing gain crossovers.
+    L = 2/(s+1)
+    ω = margin(L).wpm[]
+    m = margin(L*delay(2π/ω))
+    @test m.pm[] ≈ margin(L).pm[] atol=0.02
+
+    # Discrete-time delay margins are measured in sample intervals.
+    Ts = 0.2
+    Ld = tf([0.5], [1, -1], Ts)
+    ω = 2asin(0.25)/Ts
+    expected = (π/2 - ω*Ts/2)/(ω*Ts)
+    @test delaymargin(Ld) ≈ expected rtol=1e-3
+
+    # The smallest phase margin and smallest delay occur at different crossovers.
+    L = 0.5/(s^2+0.2s+1) + 20/(s^2+2s+100)
+    @test isstable(feedback(L))
+    @test length(margin(L; allMargins=true).wpm[]) == 4
+    @test margin(L).wpm[] ≈ 1.16742 rtol=1e-3
+    τ = delaymargin(L)
+    @test τ ≈ 0.15874 rtol=1e-3
+    @test isstable(feedback(L*pade(0.95τ, 6)))
+    @test !isstable(feedback(L*pade(1.05τ, 6)))
+
+    # Select the margin closest to zero, rather than the most negative margin.
+    m = margin(-L)
+    @test m.pm[] ≈ -9.37304 atol=0.02
+    @test m.wpm[] ≈ 0.62356 rtol=1e-3
+
+    @test delaymargin(tf(0.1, [1, 1])) == Inf
+    @test delaymargin(tf(1, [1, 1])) == Inf
+    @test delaymargin(tf(-1, [1, 1])) == 0
+end