Add phase adjustment option to margins

Introduces an adjust_phase_start flag to margin and sisomargin functions to normalize phase start values based on integrator excess.
Updates plotting functions and test cases to reflect these changes.

Author: baggepinnen

lib/ControlSystemsBase/src/analysis.jl

@@ -460,16 +460,17 @@ function relative_gain_array(A::AbstractMatrix; tol = 1e-15)
 end
 
 """
-    wgm, gm, wpm, pm = margin(sys::LTISystem, w::Vector; full=false, allMargins=false)
+    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
 
-If `!allMargins`, return only the smallest margin
+- If `!allMargins`, return only the smallest margin
+- If `full` return also `fullPhase`
+- `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.
 
-If `full` return also `fullPhase`
 See also [`delaymargin`](@ref) and [`RobustAndOptimalControl.diskmargin`](https://juliacontrol.github.io/RobustAndOptimalControl.jl/dev/api/#RobustAndOptimalControl.diskmargin)
 """
-function margin(sys::LTISystem, w::AbstractVector{<:Real}; full=false, allMargins=false)
+function margin(sys::LTISystem, w::AbstractVector{<:Real}; full=false, allMargins=false, adjust_phase_start=true)
     ny, nu = size(sys)
 
     T = float(numeric_type(sys))
@@ -488,7 +489,7 @@ function margin(sys::LTISystem, w::AbstractVector{<:Real}; full=false, allMargin
     end
     for j=1:nu
         for i=1:ny
-            wgm[i,j], gm[i,j], wpm[i,j], pm[i,j], fullPhase[i,j] = sisomargin(sys[i,j], w; full=true, allMargins)
+            wgm[i,j], gm[i,j], wpm[i,j], pm[i,j], fullPhase[i,j] = sisomargin(sys[i,j], w; full=true, allMargins, adjust_phase_start)
         end
     end
     if full
@@ -499,11 +500,11 @@ function margin(sys::LTISystem, w::AbstractVector{<:Real}; full=false, allMargin
 end
 
 """
-    ωgm, gm, ωpm, pm = sisomargin(sys::LTISystem, w::Vector; full=false, allMargins=false)
+    ωgm, gm, ωpm, pm = sisomargin(sys::LTISystem, w::Vector; full=false, allMargins=false, adjust_phase_start=true))
 
 Return frequencies for gain margins, gain margins, frequencies for phase margins, phase margins. If `allMargins=false`, only the smallest margins are returned.
 """
-function sisomargin(sys::LTISystem, w::AbstractVector{<:Real}; full=false, allMargins=false)
+function sisomargin(sys::LTISystem, w::AbstractVector{<:Real}; full=false, allMargins=false, adjust_phase_start=true)
     ny, nu = size(sys)
     if ny !=1 || nu != 1
         error("System must be SISO, use `margin` instead")
@@ -544,8 +545,19 @@ function sisomargin(sys::LTISystem, w::AbstractVector{<:Real}; full=false, allMa
             fullPhase = interpolate(fi, phase)
         end
     end
+    if adjust_phase_start && isrational(sys)
+        intexcess = integrator_excess(sys)
+        if intexcess != 0
+            # Snap phase so that it starts at -90*intexcess
+            nineties = round(Int, phase[1] / 90)
+            adjust = ((90*(-intexcess-nineties)) ÷ 360) * 360
+            pm = pm .+ adjust
+            phase .+= adjust
+            fullPhase = fullPhase .+ adjust
+        end
+    end
     if full
-        (; wgm, gm, wpm, pm, fullPhase)
+        (; wgm, gm, wpm, pm, fullPhase, phasedata = phase[:]) # phasedata is used by marginplot
     else
         (; wgm, gm, wpm, pm)
     end

lib/ControlSystemsBase/src/plotting.jl

@@ -365,7 +365,7 @@ end
     grid   --> true
     yscale --> _PlotScaleFunc
     xscale --> :log10
-    yguide --> "Magnitude"
+    yguide --> "Magnitude $_PlotScaleStr"
     x := w
     y := magdata
     ()
@@ -738,7 +738,7 @@ marginplot
     s2i(i,j) = LinearIndices((nu,(plotphase ? 2 : 1)*ny))[j,i]
     layout --> ((plotphase ? 2 : 1)*ny, nu)
     titles = Array{AbstractString}(undef, nu,ny,2,2)
-    titles[:,:,1,1] .= "Gm: "
+    titles[:,:,1,1] .= _PlotScale == "dB" ? "Gm (dB): " : "Gm: "
     titles[:,:,2,1] .= "Pm: "
     titles[:,:,1,2] .= "ω gm: "
     titles[:,:,2,2] .= "ω pm: "
@@ -750,14 +750,9 @@ marginplot
         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)  
-                # Let's be reasonable, only plot 5 smallest gain margins
+                wgm, gm, wpm, pm, fullPhase, phasedata = sisomargin(s[i,j],w; full=true, allMargins=true, adjust_phase_start)
                 if length(gm) > 5
                     @warn "Only showing smallest 5 out of $(length(gm)) gain margins"
                     idx = sortperm(gm)
@@ -775,14 +770,14 @@ marginplot
                     mag = 20 .* log10.(1 ./ gm)
                     oneLine = 0
                     @. bmag = 20*log10(bmag)
+                    titles[j,i,1,1] *= "["*join([Printf.@sprintf("%3.2g",20log10(v)) for v in gm],", ")*"] "
                 else
                     mag = 1 ./ gm
                     oneLine = 1
+                    titles[j,i,1,1] *= "["*join([Printf.@sprintf("%3.2g",v) for v in gm],", ")*"] "
                 end
-                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)
@@ -799,25 +794,21 @@ marginplot
                 end
 
                 #Plot gain margins
-                primary --> false
                 @series begin
+                    primary := false
                     color --> :gray
                     linestyle --> :dash
                     [w[1],w[end]], [oneLine,oneLine]
                 end
                 @series begin
+                    primary := false
                     [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
+
+                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],", ")*"] "
 
                 # Phase margins
                 subplot := s2i(2i,j)
@@ -830,12 +821,14 @@ marginplot
                     w, phasedata
                 end
                 @series begin
+                    primary := false
                     color --> :gray
                     linestyle --> :dash
                     seriestype := :hline
                     ((fullPhase .- pm) .* ones(1, 2))'
                 end
                 @series begin
+                    primary := false
                     [wpm wpm]', [fullPhase fullPhase-pm]'
                 end
             end

lib/ControlSystemsBase/test/framework.jl

@@ -6,7 +6,7 @@ using Plots
 gr()
 default(show=false)
 
-using ControlSystemsBase
+using ControlSystemsBase, LinearAlgebra, Test
 # Local definition to make sure we get warnings if we use eye
 eye_(n) = Matrix{Int64}(I, n, n)
 

lib/ControlSystemsBase/test/test_analysis.jl

@@ -285,6 +285,21 @@ nintbig = ControlSystemsBase.count_integrators(big(1.0)*pade(OL, 2))
 @test ControlSystemsBase.count_integrators(pade(OL, 4)) == nintbig
 @test ControlSystemsBase.count_integrators(pade(OL, 10)) == nintbig
 
+##
+G = let # From https://juliacomputing.github.io/JuliaSimControl.jl/v0.10/examples/mtk_disturbance_modeling/#Controller-reduction
+     marginplottestA = [-0.8778418152658126 -0.5578161193611026 0.6099186362718944 -0.739076573488832 0.0 -0.7985033210421671 -0.6437387074246465 -1.09990118296936 -0.9415890954623862 -0.6105710677920562 -0.40609522036480955 -0.02167670496721598 -0.0922028349547116 -0.3659595949709856 0.0 0.0 0.0 0.0; -0.5578161193611026 -0.8220564196754826 -0.4879223680823921 0.8803386749291978 0.0 -0.5305709989906725 -0.42546859154084343 -0.7310995753661961 -0.6252826290802145 -0.4179592857573079 -0.274638775913742 -0.00903355930082124 -0.05630074104219519 -0.24851140114082376 0.0 0.0 0.0 0.0; -12.772894974281625 7.422755287471556 -8.893245512259899 -0.5983738877209197 4.4676642940721933e-7 -2.901639071283097 -2.3556062284249624 -3.998192111860942 -3.428202120843976 -2.1327661674537324 -1.4414918123884637 -0.11684112979233667 -0.37069980312317624 -1.2936533657321307 0.0 0.0 0.0 0.0; 9.260923426511168 -10.119661325070803 1.5927557243537434 -6.10662600432525 0.0 -2.4254235921784115 -1.9671309526934084 -3.3286180099265987 -2.8499659817055276 -1.7302731969932739 -1.1837682861777055 -0.11782983264763462 -0.3265440770990644 -1.0546003416785925 0.0 0.0 0.0 0.0; -1.6061109633450728 0.3963654501432292 -8.331365272058026 -5.264727157230805 -0.001 -7.627995401504769 -6.199926026836587 -10.51167941872737 -9.015447054009817 -5.5777954205960585 -3.778335116508615 -0.3202631508161873 -0.9869003787621273 -3.388660360774488 0.0 0.0 0.0 0.0; 0.0 0.0 0.0 0.0 0.0 -3.548140511799198 -2.6482424961850004 -7.102082038003319 -6.208194955680467 0.0 0.0 0.0 0.0 0.0 -3.548140511799198 -2.6482424961850004 -8.102082038003319 -6.208194955680467; 0.0 0.0 0.0 0.0 0.0 -2.648242496185001 -2.0513640329992824 -5.597632716308486 -3.3299392371108265 0.0 0.0 0.0 0.0 0.0 -2.648242496185001 -2.0513640329992824 -5.597632716308486 -4.3299392371108265; 0.0 0.0 0.0 0.0 0.0 -18.102082038003317 4.4023672836915155 -24.873852981120933 -13.163260306077984 2.3828136105535203 2.0893223444460514 2.283219179019077 2.191129612074663 0.9999995532335706 -8.102082038003319 -5.5976327163084845 -21.873852981120933 -16.163260306077984; 0.0 0.0 0.0 0.0 0.0 3.7918050443195312 -14.329939237110827 -13.16326030607799 -15.408656206873678 0.0 0.0 0.0 0.0 0.0 -6.208194955680469 -4.3299392371108265 -16.16326030607799 -12.408656206873678; 0.0 0.0 0.0 0.0 0.0 -2.350321265186566 -1.6840654718182213 -5.594310418261385 -4.101569094377303 -1.488412883057869 -0.9639113397259123 0.5882419313046784 -0.8312794084435436 -0.3659595949709856 -1.5518179441443989 -1.040326764393575 -4.494409235292025 -3.159979998914917; 0.0 0.0 0.0 0.0 0.0 -2.2279953968524775 -1.5507658631630432 -5.630031465101207 -4.120845772910192 -0.9757754051184105 -1.0966951955892246 -0.4969559273832133 0.8240379338870026 -0.24851140114082376 -1.6974243978618049 -1.1252972716221998 -4.898931889735011 -3.4955631438299775; 0.0 0.0 0.0 0.0 0.0 -3.8534645754285197 -3.020606376036092 -6.73680504859119 -5.278229500140075 -14.905661141735358 5.981263475083093 -9.010086642052235 -0.969073690844096 -1.2936529189657013 -0.9518255041454229 -0.6650001476111296 -2.7386129367302487 -1.850027379296099; 0.0 0.0 0.0 0.0 0.0 -4.441954983917959 -3.3488668822504266 -9.320785111635601 -6.79570619414841 7.530650229517894 -11.303429611248509 1.4749258917061088 -6.433170081424314 -1.0546003416785925 -2.016531391739547 -1.381735929557018 -5.9921671017090015 -3.945740212442883; 0.0 0.0 0.0 0.0 0.0 -24.18765275168101 -17.451102917064617 -57.03910030189987 -42.6620899621136 -7.183906383941132 -3.3819696663653858 -8.651628422874213 -6.251627535992933 -3.3896603607744877 -16.55965735017624 -11.251176890228031 -46.5274208831725 -33.64664290810378; 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 1.0 0.0; 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 1.0; 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 -10.0 10.0 -3.0 3.0; 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 10.0 -10.0 3.0 -3.0]
+     marginplottestB = [0.0; 0.0; 0.0; 0.0; 0.0; 0.0; 0.0; 0.0; 0.0; 0.0; 0.0; 0.0; 0.0; 0.0; 0.0; 0.0; 1.0; 0.0;;]
+     marginplottestC = [2.3828136105535203 2.0893223444460514 2.283219179019077 2.191129612074663 0.9999995532335706 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0]
+     marginplottestD = [0.0;;]
+     ss(marginplottestA, marginplottestB, marginplottestC, marginplottestD)
+end
+
+marginplot(G)
+ωgₘ, gₘ, ωϕₘ, ϕₘ = margin(G, allMargins=true)
+@test ωgₘ[] ≈ [0.9788420039794175, 21.96146912516854]
+@test gₘ[] ≈ [0.19398511072183675, 11.886085245062574]
+@test ωϕₘ[][] ≈ 3.484166418318219
+@test ϕₘ[][] ≈ 50.783187269018754
 
 # RGA
 a = 10

lib/ControlSystemsBase/test/test_plots.jl

@@ -60,4 +60,6 @@ end
   @test_nowarn plot(step(Gmimo, 10), plotx=true)
   # plot!(step(Gmimo[:, 1], 10), plotx=true) # Verify that this plots the same as the "from u(1)" series above
 
+  setPlotScale("log10")
+  margingen()
 end