From 836c11cf850aa93c7d1fba11e4ce6ef7fb132c5c Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Fri, 10 Jan 2025 16:40:39 +0100 Subject: [PATCH 1/2] add tools for counting multiple eigvals and adjust bode phase start --- lib/ControlSystemsBase/src/analysis.jl | 41 +++++++++++++++++++ lib/ControlSystemsBase/src/plotting.jl | 12 +++++- lib/ControlSystemsBase/src/simplification.jl | 6 ++- .../src/types/conversion.jl | 8 +++- lib/ControlSystemsBase/test/test_analysis.jl | 24 +++++++++++ 5 files changed, 88 insertions(+), 3 deletions(-) diff --git a/lib/ControlSystemsBase/src/analysis.jl b/lib/ControlSystemsBase/src/analysis.jl index a1bd6c003..f785d74cd 100644 --- a/lib/ControlSystemsBase/src/analysis.jl +++ b/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. """ diff --git a/lib/ControlSystemsBase/src/plotting.jl b/lib/ControlSystemsBase/src/plotting.jl index db3de011b..35d676fbe 100644 --- a/lib/ControlSystemsBase/src/plotting.jl +++ b/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 + 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 diff --git a/lib/ControlSystemsBase/src/simplification.jl b/lib/ControlSystemsBase/src/simplification.jl index 103aac075..efa21ec79 100644 --- a/lib/ControlSystemsBase/src/simplification.jl +++ b/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 diff --git a/lib/ControlSystemsBase/src/types/conversion.jl b/lib/ControlSystemsBase/src/types/conversion.jl index b5f26d7b1..0eb8cc5c0 100644 --- a/lib/ControlSystemsBase/src/types/conversion.jl +++ b/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 """ diff --git a/lib/ControlSystemsBase/test/test_analysis.jl b/lib/ControlSystemsBase/test/test_analysis.jl index 444c46a6e..23cd6951b 100644 --- a/lib/ControlSystemsBase/test/test_analysis.jl +++ b/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 \ No newline at end of file From 96a92f8652501dfbc0688372553fe184bc58846b Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Sat, 11 Jan 2025 06:51:33 +0100 Subject: [PATCH 2/2] only adjust phase for rational systems --- lib/ControlSystemsBase/src/plotting.jl | 2 +- lib/ControlSystemsBase/src/types/Lti.jl | 1 + lib/ControlSystemsBase/src/types/StateSpace.jl | 1 + lib/ControlSystemsBase/src/types/TransferFunction.jl | 2 ++ 4 files changed, 5 insertions(+), 1 deletion(-) diff --git a/lib/ControlSystemsBase/src/plotting.jl b/lib/ControlSystemsBase/src/plotting.jl index 35d676fbe..e9207585d 100644 --- a/lib/ControlSystemsBase/src/plotting.jl +++ b/lib/ControlSystemsBase/src/plotting.jl @@ -329,7 +329,7 @@ end end plotphase || continue - if adjust_phase_start == true + if adjust_phase_start == true && isrational(sbal) intexcess = integrator_excess(sbal) if intexcess != 0 # Snap phase so that it starts at -90*intexcess diff --git a/lib/ControlSystemsBase/src/types/Lti.jl b/lib/ControlSystemsBase/src/types/Lti.jl index 4f74f833d..546418104 100644 --- a/lib/ControlSystemsBase/src/types/Lti.jl +++ b/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) diff --git a/lib/ControlSystemsBase/src/types/StateSpace.jl b/lib/ControlSystemsBase/src/types/StateSpace.jl index 98e8006a9..50ba042eb 100644 --- a/lib/ControlSystemsBase/src/types/StateSpace.jl +++ b/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 ## diff --git a/lib/ControlSystemsBase/src/types/TransferFunction.jl b/lib/ControlSystemsBase/src/types/TransferFunction.jl index f8bb1e307..ff2b2fbe7 100644 --- a/lib/ControlSystemsBase/src/types/TransferFunction.jl +++ b/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 ## #####################################################################