handle phase wrapping in margin

Author: baggepinnen

lib/ControlSystemsBase/src/analysis.jl

@@ -496,12 +496,18 @@ function sisomargin(sys::LTISystem, w::AbstractVector{<:Real}; full=false, allMa
     wgm, = _allPhaseCrossings(w, phase)
     gm = similar(wgm)
     for i = eachindex(wgm)
-        gm[i] = 1 ./ abs(freqresp(sys,[wgm[i]])[1][1])
+        gm[i] = 1 ./ abs(freqresp(sys,wgm[i])[1])
     end
     wpm, fi = _allGainCrossings(w, mag)
     pm = similar(wpm)
     for i = eachindex(wpm)
-        pm[i] = mod(rad2deg(angle(freqresp(sys,[wpm[i]])[1][1])),360)-180
+        # 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])[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
     end
     if !allMargins #Only output the smallest margins
         gm, idx = findmin([gm;Inf])

lib/ControlSystemsBase/src/plotting.jl

@@ -771,10 +771,10 @@ marginplot
                     mag = 1 ./ gm
                     oneLine = 1
                 end
-                titles[j,i,1,1] *= "["*join([Printf.@sprintf("%2.2f",v) for v in gm],", ")*"] "
-                titles[j,i,1,2] *= "["*join([Printf.@sprintf("%2.2f",v) for v in wgm],", ")*"] "
-                titles[j,i,2,1] *=  "["*join([Printf.@sprintf("%2.2f",v) for v in pm],", ")*"] "
-                titles[j,i,2,2] *=  "["*join([Printf.@sprintf("%2.2f",v) for v in wpm],", ")*"] "
+                titles[j,i,1,1] *= "["*join([Printf.@sprintf("%3.2g",v) for v in gm],", ")*"] "
+                titles[j,i,1,2] *= "["*join([Printf.@sprintf("%3.2g",v) for v in wgm],", ")*"] "
+                titles[j,i,2,1] *=  "["*join([Printf.@sprintf("%3.2g",v) for v in pm],", ")*"] "
+                titles[j,i,2,2] *=  "["*join([Printf.@sprintf("%3.2g",v) for v in wpm],", ")*"] "
 
                 subplot := min(s2i((plotphase ? (2i-1) : i),j), prod(plotattributes[:layout]))
                 if si == length(systems)

lib/ControlSystemsBase/test/test_analysis.jl

@@ -250,6 +250,25 @@ P = tf(1,[5, 10.25, 6.25, 1])
 w_180, gm, w_c, pm = margin(50P)
 @test pm[] ≈ -35.1 rtol=1e-2
 
+## Tricky case from https://www.reddit.com/r/ControlTheory/comments/1inhxsz/understanding_stability_in_highorder/
+s = tf("s")
+kpu = -10.593216768722073; kiu = -0.00063; t = 1000; tau = 180; a = 1/8.3738067325406132e-5;
+kpd = 15.92190277847431; kid = 0.000790960718241793;
+kpo = -10.39321676872207317; kio = -0.00063;
+kpb = kpd; kib = kid;
+
+C1 = (kpu + kiu/s)*(1/(t*s + 1))
+C2 = (kpu + kiu/s)*(1/(t*s + 1))
+C3 = (kpo + kio/s)*(1/(t*s + 1))
+Cb = (kpb + kib/s)*(1/(t*s + 1))
+OL = (ss(Cb)*ss(C1)*ss(C2)*ss(C3)*exp(-3*tau*s))/((C1 - a*s)*(C2 - a*s)*(C3 - a*s));
+
+wgm, gm, ωϕₘ, ϕₘ = margin(OL; full=true, allMargins=true)
+@test ϕₘ[][] ≈ -320 rtol=1e-2
+for wgm in wgm[]
+     @test mod(rad2deg(angle(freqresp(OL, wgm)[])), 360)-180 ≈ 0 atol=1e-1
+end
+
 # RGA
 a = 10
 P = ss([0 a; -a 0], I(2), [1 a; -a 1], 0)