add tools for counting multiple eigvals and adjust bode phase start (#950)

* add tools for counting multiple eigvals and adjust bode phase start

* only adjust phase for rational systems

Author: baggepinnen

lib/ControlSystemsBase/src/analysis.jl

@@ -37,6 +37,47 @@ function poles(sys::TransferFunction{<:TimeEvolution,SisoZpk{T,TR}}) where {T, T
     return lcmpoles
 end
 
+
+function count_eigval_multiplicity(p, location)
+    T = float(real(eltype(p)))
+    n = length(p)
+    n == 0 && return 0
+    maximum(1:n) do i
+        # if we count i poles within the circle assuming i integrators, we return i
+        if count(p->abs(p-location) < (i+1)*(eps(T)^(1/i)), p) >= i
+            i
+        else
+            0
+        end
+    end
+end
+
+"""
+    count_integrators(P)
+
+Count the number of poles in the origin by finding the maximum value of `n` for which the number of poles within a circle of radius `(n+1)*eps(numeric_type(sys))^(1/n)` arount the origin (1 in discrete time) equals `n`.
+"""
+function count_integrators(P::LTISystem)
+    p = poles(P)
+    location = iscontinuous(P) ? 0 : 1
+    count_eigval_multiplicity(p, location)
+end
+
+"""
+    integrator_excess(P)
+
+Count the number of integrators in the system by finding the difference between the number of poles in the origin and the number of zeros in the origin. If the number of zeros in the origin is greater than the number of poles in the origin, the count is negative.
+
+For discrete-tiem systems, the origin ``s = 0`` is replaced by the point ``z = 1``.
+"""
+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)
+end
+
+
 # TODO: Improve implementation, should be more efficient ways.
 # Calculates the same minors several times in some cases.
 """

lib/ControlSystemsBase/src/plotting.jl

@@ -253,6 +253,7 @@ optionally provided. To change the Magnitude scale see [`setPlotScale`](@ref). T
                                             
 - If `hz=true`, the plot x-axis will be displayed in Hertz, the input frequency vector is still treated as rad/s.
 - `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.
 """
@@ -274,7 +275,7 @@ function _get_plotlabel(s, i, j)
     end
 end
 
-@recipe function bodeplot(p::Bodeplot; plotphase=true, ylimsphase=(), unwrap=true, hz=false, balance=true)
+@recipe function bodeplot(p::Bodeplot; plotphase=true, ylimsphase=(), unwrap=true, hz=false, balance=true, adjust_phase_start=true)
     systems, w = _processfreqplot(Val{:bode}(), p.args...)
     ws = (hz ? 1/(2π) : 1) .* w
     ny, nu = size(systems[1])
@@ -328,6 +329,15 @@ end
                 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)
+                        phasedata .+= ((90*(-intexcess-nineties)) ÷ 360) * 360
+                    end
+                end
+
                 @series begin
                     xscale    --> :log10
                     # ylims      := ylimsphase

lib/ControlSystemsBase/src/simplification.jl

@@ -47,7 +47,11 @@ function struct_ctrb_states(A::AbstractVecOrMat, B::AbstractVecOrMat)
     # UInt16 can only store up to 65535, so if A is completely dense and of size larger than 65535, the computations below might overflow. This is exceedingly unlikely though.
     bitA = UInt16.(.!iszero.(A)) # Convert to Int because multiplying with a bit matrix is slow
     x = vec(any(B .!= 0, dims=2)) # index vector indicating states that have been affected by input
-    xi = bitA * x
+    if A isa SMatrix
+        xi = Vector(bitA * x) # To aboid setindex! error below
+    else
+        xi = bitA * x
+    end
     xi2 = similar(xi)
     @. xi = (xi != false) | !iszero(x)
     for i = 2:size(A, 1) # apply A nx times, similar to controllability matrix

lib/ControlSystemsBase/src/types/Lti.jl

@@ -1,4 +1,5 @@
 abstract type LTISystem{TE<:TimeEvolution} <: AbstractSystem end
+isrational(sys::LTISystem) = false
 +(sys1::LTISystem, sys2::LTISystem) = +(promote(sys1, sys2)...)
 -(sys1::LTISystem, sys2::LTISystem) = -(promote(sys1, sys2)...)
 function *(sys1::LTISystem, sys2::LTISystem)

lib/ControlSystemsBase/src/types/StateSpace.jl

@@ -234,6 +234,7 @@ function isstable(sys::StateSpace{<:Discrete, <:ForwardDiff.Dual})
     all(abs.(ForwardDiff.value.(sys.A)) .< 1)
 end
 
+isrational(::AbstractStateSpace) = true
 
 #####################################################################
 ##                         Math Operators                          ##

lib/ControlSystemsBase/src/types/TransferFunction.jl

@@ -98,6 +98,8 @@ function isproper(G::TransferFunction)
     return all(isproper(f) for f in G.matrix)
 end
 
+isrational(::TransferFunction) = true
+
 #####################################################################
 ##                         Math Operators                          ##
 #####################################################################

lib/ControlSystemsBase/src/types/conversion.jl

@@ -337,7 +337,13 @@ function siso_ss_to_zpk(sys, i, j)
     nx = size(A, 1)
     nz = length(z)
     k = nz == nx ? D[1] : (C*(A^(nx - nz - 1))*B)[1]
-    return z, eigvals(A), k
+    if A isa SMatrix
+        # Only hermitian matrices are diagonalizable by *StaticArrays*. Non-Hermitian matrices should be converted to `Array` first.
+        p = eigvals(Matrix(A))
+    else
+        p = eigvals(A)
+    end
+    return z, p, k
 end
 
 """

lib/ControlSystemsBase/test/test_analysis.jl

@@ -16,6 +16,7 @@ D = [1 0;
      1 0]
 
 ex_3 = ss(A, B, C, D)
+@test ControlSystemsBase.count_integrators(ex_3) == 6
 @test tzeros(ex_3) ≈ [0.3411639019140099 + 1.161541399997252im,
                              0.3411639019140099 - 1.161541399997252im,
                              0.9999999999999999 + 0.0im,
@@ -36,12 +37,14 @@ C = [1 0 0 0 0;
 D = zeros(2, 2)
 ex_4 = ss(A, B, C, D)
 @test tzeros(ex_4) ≈ [-0.06467751189940692,-0.3680512036036696]
+@test ControlSystemsBase.count_integrators(ex_4) == 1
 
 # Example 5
 s = tf("s")
 ex_5 = 1/s^15
 @test tzeros(ex_5) == Float64[]
 @test tzeros(ss(ex_5)) == Float64[]
+@test ControlSystemsBase.count_integrators(ex_5) == 15
 
 # Example 6
 A = [2 -1 0;
@@ -52,6 +55,7 @@ C = [0 -1 0]
 D = [0]
 ex_6 = ss(A, B, C, D)
 @test tzeros(ex_6) == Float64[]
+@test ControlSystemsBase.count_integrators(ex_6) == 2
 
 @test ss(A, [0 0 1]', C, D) == ex_6
 
@@ -72,6 +76,7 @@ D = [0]
 ex_8 = ss(A, B, C, D)
 # TODO : there may be a way to improve the precision of this example.
 @test tzeros(ex_8) ≈ [-1.0, -1.0] atol=1e-7
+@test ControlSystemsBase.count_integrators(ex_8) == 0
 
 # Example 9
 ex_9 = (s - 20)/s^15
@@ -121,6 +126,7 @@ approxin2(el,col) = any(el.≈col)
 
 ex_12 = ss(-3, 2, 1, 2)
 @test poles(ex_12) ≈ [-3]
+@test ControlSystemsBase.count_integrators(ex_12) == 0
 
 ex_13 = ss([-1 1; 0 -1], [0; 1], [1 0], 0)
 @test poles(ex_13) ≈ [-1, -1]
@@ -307,4 +313,22 @@ gof = gangoffour(P,C)
 F = ssrand(2,2,2,proper=true)
 @test_nowarn gangofseven(P, C, F)
 
+
+## Approximate double integrator
+P = let
+     tempA = [
+          0.0 0.0 1.0 0.0 0.0 0.0
+          0.0 0.0 0.0 0.0 1.0 0.0
+          -400.0 400.0 -0.4 -0.0 0.4 -0.0
+          0.0 0.0 0.0 0.0 0.0 1.0
+          10.0 -11.0 0.01 1.0 -0.011 0.001
+          0.0 10.0 0.0 -10.0 0.01 -0.01
+     ]
+     tempB = [0.0 0.0; 0.0 0.0; 100.80166347371734 0.0; 0.0 0.0; 0.0 -0.001; 0.0 0.01]
+     tempC = [0.0 0.0 0.0 1.0 0.0 0.0]
+     tempD = [0.0 0.0]
+     ss(tempA, tempB, tempC, tempD)
+end
+@test ControlSystemsBase.count_integrators(P) == 2
+
 end