handle RHP poles in margin

lib/ControlSystemsBase/src/analysis.jl

@@ -45,14 +45,16 @@ end
 
 function count_eigval_multiplicity(p, location, e=eps(maximum(abs, p, init=0.0))) # The init is to handle poor type inference with exotic number types
     n = length(p)
-    n == 0 && return 0
+    tol = zero(e)
+    n == 0 && return (0, tol)
     for i = 1:n
         # if we count i poles within the circle assuming i integrators, we return i
-        if count(p->abs(p-location) < (e^(1/i)), p) == i
-            return i
+        tol = e^(1/i)
+        if count(p->abs(p-location) < tol, p) == i
+            return (i, tol)
         end
     end
-    0
+    (0, tol)
 end
 
 """
@@ -65,7 +67,7 @@ See also [`integrator_excess`](@ref).
 function count_integrators(P::LTISystem)
     p = poles(P)
     location = iscontinuous(P) ? 0 : 1
-    count_eigval_multiplicity(p, location)
+    count_eigval_multiplicity(p, location)[1]
 end
 
 """
@@ -79,7 +81,18 @@ function integrator_excess(P::LTISystem)
     p = poles(P)
     z = tzeros(P)
     location = iscontinuous(P) ? 0 : 1
-    count_eigval_multiplicity(p, location) - count_eigval_multiplicity(z, location)
+    np, tolp = count_eigval_multiplicity(p, location)
+    nz, tolz = count_eigval_multiplicity(z, location)
+    np - nz
+end
+
+function integrator_excess_with_tol(P::LTISystem)
+    p = poles(P)
+    z = tzeros(P)
+    location = iscontinuous(P) ? 0 : 1
+    np, tolp = count_eigval_multiplicity(p, location)
+    nz, tolz = count_eigval_multiplicity(z, location)
+    np - nz, p, z, tolp, tolz
 end
 
 
@@ -563,12 +576,14 @@ function sisomargin(sys::LTISystem, w::AbstractVector{<:Real}; full=false, allMa
         end
     end
     if adjust_phase_start && isrational(sys)
-        intexcess = integrator_excess(sys)
+        intexcess, p, z, tol = integrator_excess_with_tol(sys)
+        n_unstable_poles = count(real(p) > tol for p in p)
         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
+            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

lib/ControlSystemsBase/test/test_analysis.jl

@@ -303,6 +303,18 @@ marginplot(G)
 @test ωϕₘ[][] ≈ 3.484166418318219
 @test ϕₘ[][] ≈ 50.783187269018754
 
+
+## System with RHP poles and zeros but stable in closed loop
+temp = let
+    tempA = [-83.25487376152321 26.76511464761918 -43.73380610734441 -27.90719213203418 -9.51838201282524 21.932907840445864; -15.234225958115879 -7.005196625072119 -18.09631269583633 -1.5182977493068577 -18.323658349439512 32.32164964342026; 0.0 23.7278286542071 -2.198121109985474 -17.293997884135962 19.215332126022005 -31.45685591523796; 0.0 0.0 -20.278223915399145 -9.092020460145815 -11.728147286799778 21.40857710364235; 0.0 0.0 0.0 -0.4760794287541981 -3.7362512320067136 5.668501934932698; 0.0 0.0 0.0 0.0 -2.469694372034253 3.7916178279085737]
+    tempB = [9.65940730736899; 0.0; 0.0; 0.0; 0.0; 0.0;;]
+    tempC = [0.0 4.9399258118993306 -5.6197700696954245 -4.845153904367337 0.28951168477387096 0.28822873250094244]
+    tempD = [0.0;;]
+    ss(tempA, tempB, tempC, tempD)
+end
+ωgₘ, gₘ, ωϕₘ, ϕₘ = margin(temp, allMargins=true)
+@test ϕₘ[][] ≈ 27.406506322937503
+
 # RGA
 a = 10
 P = ss([0 a; -a 0], I(2), [1 a; -a 1], 0)