Merge pull request #969 from JuliaControl/phasemargfix

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/connections.jl

@@ -215,6 +215,10 @@ function /(sys1::Union{StateSpace,AbstractStateSpace}, sys2::LTISystem)
     sys1new, sys2new = promote(sys1, 1/sys2)
     return sys1new*sys2new
 end
+function /(sys1::Union{StateSpace,AbstractStateSpace}, sys2::TransferFunction) # This method is handling ambiguity between method above and one with explicit TF as second argument, hit by ss(1)/tf(1)
+    sys1new, sys2new = promote(sys1, 1/sys2)
+    return sys1new*sys2new
+end
 
 @static if VERSION >= v"1.8.0-beta1"
     blockdiag(anything...) = cat(anything..., dims=Val((1,2)))

lib/ControlSystemsBase/src/pid_design.jl

@@ -150,8 +150,8 @@ r  ┌─────┐     ┌─────┐          │      │    │
 ```
 
 The `form` can be chosen as one of the following (determines how the arguments `param_p, param_i, param_d` are interpreted)
-* `:standard` - ``K_p*(br-y + (r-y)/(T_i s) + T_d s (cr-y)/(T_f s + 1))``
-* `:parallel` - ``K_p*(br-y) + K_i (r-y)/s + K_d s (cr-y)/(Tf s + 1)``
+* `:standard` - ``K_p(br-y + (r-y)/(T_i s) + T_d s (cr-y)/(T_f s + 1))``
+* `:parallel` - ``K_p(br-y) + K_i (r-y)/s + K_d s (cr-y)/(Tf s + 1)``
 
 - `b` is a set-point weighting for the proportional term
 - `c` is a set-point weighting for the derivative term, this defaults to 0.

lib/ControlSystemsBase/src/plotting.jl

@@ -293,6 +293,9 @@ end
         else
             sbal = s
         end
+        if plotphase && adjust_phase_start && isrational(sbal)
+            intexcess = integrator_excess(sbal)
+        end
         mag, phase = bode(sbal, w; unwrap=false)
         if _PlotScale == "dB" # Set by setPlotScale(str) globally
             mag = 20*log10.(mag)
@@ -330,7 +333,6 @@ end
                 plotphase || continue
 
                 if adjust_phase_start == true && isrational(sbal)
-                    intexcess = integrator_excess(sbal)
                     if intexcess != 0
                         # Snap phase so that it starts at -90*intexcess
                         nineties = round(Int, phasedata[1] / 90)
@@ -429,7 +431,7 @@ nyquistplot
                     if lab !== nothing
                         label --> lab
                     end
-                    hover --> [hz ? Printf.@sprintf("f = %.3f", w/2π) : Printf.@sprintf("ω = %.3f", w) for w in w]
+                    hover --> [hz ? Printf.@sprintf("f = %.3g", w/2π) : Printf.@sprintf("ω = %.3g", w) for w in w]
                     (redata, imdata)
                 end                
 
@@ -725,11 +727,12 @@ Plot all the amplitude and phase margins of the system(s) `sys`.
 
 - A frequency vector `w` can be optionally provided.
 - `balance`: Call [`balance_statespace`](@ref) on the system before plotting.
+- `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.
 
 `kwargs` is sent as argument to RecipesBase.plot.
 """
 marginplot
-@recipe function marginplot(p::Marginplot; plotphase=true, hz=false, balance=true)
+@recipe function marginplot(p::Marginplot; plotphase=true, hz=false, balance=true, adjust_phase_start=true)
     systems, w = _processfreqplot(Val{:bode}(), p.args...)
     ny, nu = size(systems[1])
     s2i(i,j) = LinearIndices((nu,(plotphase ? 2 : 1)*ny))[j,i]
@@ -746,6 +749,11 @@ marginplot
             s = balance_statespace(s)[1]
         end
         bmag, bphase = bode(s, w)
+
+        if plotphase && adjust_phase_start && isrational(s)
+            intexcess = integrator_excess(s)
+        end
+
         for j=1:nu
             for i=1:ny
                 wgm, gm, wpm, pm, fullPhase = sisomargin(s[i,j],w, full=true, allMargins=true)  
@@ -771,10 +779,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)
@@ -801,6 +809,15 @@ marginplot
                     [wgm wgm]', [ones(length(mag)) mag]'
                 end
                 plotphase || continue
+
+                phasedata = bphase[i, j, :]
+                if plotphase && adjust_phase_start && isrational(s)
+                    if intexcess != 0
+                        # Snap phase so that it starts at -90*intexcess
+                        nineties = round(Int, phasedata[1] / 90)
+                        phasedata .+= ((90*(-intexcess-nineties)) ÷ 360) * 360
+                    end
+                end
 
                 # Phase margins
                 subplot := s2i(2i,j)
@@ -810,7 +827,7 @@ marginplot
                 @series begin
                     primary := true
                     seriestype := :bodephase
-                    w, bphase[i, j, :]
+                    w, phasedata
                 end
                 @series begin
                     color --> :gray

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)

lib/ControlSystemsBase/test/test_connections.jl

@@ -438,4 +438,6 @@ Pr = input_resolvent(P)
 @test Pr.C == I
 @test iszero(Pr.D)
 
+@test ss(1) / tf(1) == ss(1) # Test no method ambiguity
+
 end