From b799c51d0f6bbb904719bc1df01c81d354bd0e52 Mon Sep 17 00:00:00 2001 From: Tallak Tveide Date: Wed, 2 Sep 2020 08:33:28 +0200 Subject: [PATCH 1/9] Fix bode plot always negative phase If the phase was initially less than -180 degrees, the bode plot would plot with positive initial phase --- src/freqresp.jl | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/freqresp.jl b/src/freqresp.jl index 21bf882fa..536ed2c2a 100644 --- a/src/freqresp.jl +++ b/src/freqresp.jl @@ -104,7 +104,13 @@ at frequencies `w` `mag` and `phase` has size `(length(w), ny, nu)`""" function bode(sys::LTISystem, w::AbstractVector) resp = freqresp(sys, w) - return abs.(resp), rad2deg.(unwrap!(angle.(resp),1)), w + phase = rad2deg.(unwrap!(angle.(resp),1)) + offset = if first(phase > 0) + -360.0 + else + 0 + end + return abs.(resp), phase .+ offset, w end bode(sys::LTISystem) = bode(sys, _default_freq_vector(sys, Val{:bode}())) From 55eca81f7a2327b32aac8bc3fccc924c9cc019d3 Mon Sep 17 00:00:00 2001 From: Tallak Tveide Date: Thu, 3 Sep 2020 13:30:04 +0200 Subject: [PATCH 2/9] Algorithm to estimate initial phase angle for bode We use the zero poles (continuous) and poles on the unit circle (discrete) to estimate the phase angle at the left side of the bode plot. Angle unwrap starts from this angle, then the initial angle is not used anymore. --- src/freqresp.jl | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/freqresp.jl b/src/freqresp.jl index 536ed2c2a..24c21c2f0 100644 --- a/src/freqresp.jl +++ b/src/freqresp.jl @@ -104,13 +104,17 @@ at frequencies `w` `mag` and `phase` has size `(length(w), ny, nu)`""" function bode(sys::LTISystem, w::AbstractVector) resp = freqresp(sys, w) - phase = rad2deg.(unwrap!(angle.(resp),1)) - offset = if first(phase > 0) - -360.0 + # use the number of integrators in the system to estimate initial phase + count_unit_circle = x -> count(abs.(abs.(x) .- 1.0) .< 1e-5) + count_zeros = x -> count(abs.(x) .< 1e-8) + + phase0 = if sys.Ts == 0.0 + pi/2 * (count_zeros(zpk(sys).matrix[1].z) - count_zeros(zpk(sys).matrix[1].p)) else - 0 + pi/2 * (count_unit_circle(zpk(sys).matrix[1].z) - count_unit_circle(zpk(sys).matrix[1].p)) end - return abs.(resp), phase .+ offset, w + phase = rad2deg.(unwrap!(vcat(phase0, angle.(resp)),1))[2:end] + return abs.(resp), phase, w end bode(sys::LTISystem) = bode(sys, _default_freq_vector(sys, Val{:bode}())) From feecfbc203f8907cf1de8532f1e7da4b83133f3e Mon Sep 17 00:00:00 2001 From: Tallak Tveide Date: Thu, 3 Sep 2020 20:36:19 +0200 Subject: [PATCH 3/9] Allow unwrap! to start from an initial value --- src/utilities.jl | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/utilities.jl b/src/utilities.jl index e2ea1b95c..da2676d73 100644 --- a/src/utilities.jl +++ b/src/utilities.jl @@ -102,22 +102,29 @@ end poly2vec(p::Polynomial) = p.coeffs[1:end] -function unwrap!(M::Array, dim=1) +function unwrap!(M::Array, dim=1; init = 0) alldims(i) = [ n==dim ? i : (1:size(M,n)) for n in 1:ndims(M) ] - for i = 2:size(M, dim) + + for i = 1:size(M, dim) #This is a copy of slicedim from the JuliaLang but enables us to write to it #The code (with dim=1) is equivalent to # d = M[i,:,:,...,:] - M[i-1,:,...,:] # M[i,:,:,...,:] -= floor((d+π) / (2π)) * 2π - d = M[alldims(i)...] - M[alldims(i-1)...] + prev = if i > 1 + M[alldims(i-1)...] + else + fill(eltype(M)(init), size(M[alldims(1)...])) + end + d = M[alldims(i)...] - prev π2 = eltype(M)(2π) M[alldims(i)...] -= floor.((d .+ π) / π2) * π2 end return M end + #Collect will create a copy and collect the elements -unwrap(m::AbstractArray, args...) = unwrap!(collect(m), args...) +unwrap(m::AbstractArray, args...; init = 0) = unwrap!(collect(m), args..., init = init) unwrap(x::Number) = x """ From 066bce8aa2f0902977cd51b7b83d287183a24092 Mon Sep 17 00:00:00 2001 From: Tallak Tveide Date: Thu, 3 Sep 2020 20:41:54 +0200 Subject: [PATCH 4/9] Optimize bode function --- src/freqresp.jl | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/freqresp.jl b/src/freqresp.jl index 24c21c2f0..7302ac1b5 100644 --- a/src/freqresp.jl +++ b/src/freqresp.jl @@ -108,12 +108,13 @@ function bode(sys::LTISystem, w::AbstractVector) count_unit_circle = x -> count(abs.(abs.(x) .- 1.0) .< 1e-5) count_zeros = x -> count(abs.(x) .< 1e-8) + zpk_sys = zpk(sys) phase0 = if sys.Ts == 0.0 - pi/2 * (count_zeros(zpk(sys).matrix[1].z) - count_zeros(zpk(sys).matrix[1].p)) + pi/2 * (count_zeros(zpk_sys.matrix[1].z) - count_zeros(zpk_sys.matrix[1].p)) else - pi/2 * (count_unit_circle(zpk(sys).matrix[1].z) - count_unit_circle(zpk(sys).matrix[1].p)) + pi/2 * (count_unit_circle(zpk_sys.matrix[1].z) - count_unit_circle(zpk_sys.matrix[1].p)) end - phase = rad2deg.(unwrap!(vcat(phase0, angle.(resp)),1))[2:end] + phase = rad2deg.(unwrap!(angle.(resp), 1, init = phase0)) return abs.(resp), phase, w end bode(sys::LTISystem) = bode(sys, _default_freq_vector(sys, Val{:bode}())) From 32810625a8da534f59a47a5c459a2042f811d6e2 Mon Sep 17 00:00:00 2001 From: Tallak Tveide Date: Thu, 3 Sep 2020 20:52:16 +0200 Subject: [PATCH 5/9] Fix a bug in unwrap! Sometimes the use of - would not work because the operands would be a scalar and an array --- src/utilities.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/utilities.jl b/src/utilities.jl index da2676d73..3c352401c 100644 --- a/src/utilities.jl +++ b/src/utilities.jl @@ -113,9 +113,9 @@ function unwrap!(M::Array, dim=1; init = 0) prev = if i > 1 M[alldims(i-1)...] else - fill(eltype(M)(init), size(M[alldims(1)...])) + eltype(M)(init) end - d = M[alldims(i)...] - prev + d = M[alldims(i)...] .- prev π2 = eltype(M)(2π) M[alldims(i)...] -= floor.((d .+ π) / π2) * π2 end From 579acf4121fd4b957de93ad79c4a264afe8f3705 Mon Sep 17 00:00:00 2001 From: Tallak Tveide Date: Thu, 3 Sep 2020 20:55:07 +0200 Subject: [PATCH 6/9] Optimize bode function by using function `count` --- src/freqresp.jl | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/freqresp.jl b/src/freqresp.jl index 7302ac1b5..29f88648a 100644 --- a/src/freqresp.jl +++ b/src/freqresp.jl @@ -104,9 +104,10 @@ at frequencies `w` `mag` and `phase` has size `(length(w), ny, nu)`""" function bode(sys::LTISystem, w::AbstractVector) resp = freqresp(sys, w) - # use the number of integrators in the system to estimate initial phase - count_unit_circle = x -> count(abs.(abs.(x) .- 1.0) .< 1e-5) - count_zeros = x -> count(abs.(x) .< 1e-8) + + # use the number of integrators in the system to estimate initial phase at w=0 + count_unit_circle = x -> count(y -> abs(abs(y) - 1.0) < 1e-5, x) + count_zeros = x -> count(y -> abs(y) < 1e-8, x) zpk_sys = zpk(sys) phase0 = if sys.Ts == 0.0 From 22a369d3eca7fd85f33dbbb25322ad69ed11d061 Mon Sep 17 00:00:00 2001 From: Tallak Tveide Date: Thu, 3 Sep 2020 21:31:42 +0200 Subject: [PATCH 7/9] Add init=... for bodeplot phase with unwrap --- src/plotting.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/plotting.jl b/src/plotting.jl index 335042df5..c972945e5 100644 --- a/src/plotting.jl +++ b/src/plotting.jl @@ -313,7 +313,7 @@ bodeplot label --> "\$G_{$(si)}\$" linestyle --> styledict[:l] seriescolor --> styledict[:c] - w, unwrap ? ControlSystems.unwrap(phasedata.*(pi/180)).*(180/pi) : phasedata + w, unwrap ? ControlSystems.unwrap(phasedata.*(pi/180), init = deg2rad(first(phasedata))).*(180/pi) : phasedata end end From d9075a1a004f0da8d68a6bb1b435a1b698f71236 Mon Sep 17 00:00:00 2001 From: Tallak Tveide Date: Thu, 3 Sep 2020 21:44:04 +0200 Subject: [PATCH 8/9] Make bode phase estimate thresholds configurable --- src/freqresp.jl | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/freqresp.jl b/src/freqresp.jl index 29f88648a..d3903e75a 100644 --- a/src/freqresp.jl +++ b/src/freqresp.jl @@ -102,13 +102,13 @@ Compute the magnitude and phase parts of the frequency response of system `sys` at frequencies `w` `mag` and `phase` has size `(length(w), ny, nu)`""" -function bode(sys::LTISystem, w::AbstractVector) +function bode(sys::LTISystem, w::AbstractVector; unit_circle_threshold = 1e-5, zero_threshold = 1e-8) resp = freqresp(sys, w) # use the number of integrators in the system to estimate initial phase at w=0 - count_unit_circle = x -> count(y -> abs(abs(y) - 1.0) < 1e-5, x) - count_zeros = x -> count(y -> abs(y) < 1e-8, x) - + count_unit_circle = x -> count(y -> abs(abs(y) - 1.0) < unit_circle_threshold, x) + count_zeros = x -> count(y -> abs(y) < zero_threshold, x) + zpk_sys = zpk(sys) phase0 = if sys.Ts == 0.0 pi/2 * (count_zeros(zpk_sys.matrix[1].z) - count_zeros(zpk_sys.matrix[1].p)) @@ -118,7 +118,7 @@ function bode(sys::LTISystem, w::AbstractVector) phase = rad2deg.(unwrap!(angle.(resp), 1, init = phase0)) return abs.(resp), phase, w end -bode(sys::LTISystem) = bode(sys, _default_freq_vector(sys, Val{:bode}())) +bode(sys::LTISystem; kwargs...) = bode(sys, _default_freq_vector(sys, Val{:bode}()); kwargs...) """`re, im, w = nyquist(sys[, w])` From dab68bc41ce55f751aa6cd2d0c955920aeae4dd3 Mon Sep 17 00:00:00 2001 From: Tallak Tveide Date: Fri, 4 Sep 2020 20:45:50 +0200 Subject: [PATCH 9/9] Bode plot frequency disabled for DelayLtiSystem The heuristic used to find the initial phase at w=0 for bode plots does not work for systems of type DelayLtiSystem. So it will default to zero initial phase (which is wrong for many systems, but at least it doesn't blow up). --- src/freqresp.jl | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/freqresp.jl b/src/freqresp.jl index d3903e75a..f5f676261 100644 --- a/src/freqresp.jl +++ b/src/freqresp.jl @@ -109,11 +109,15 @@ function bode(sys::LTISystem, w::AbstractVector; unit_circle_threshold = 1e-5, z count_unit_circle = x -> count(y -> abs(abs(y) - 1.0) < unit_circle_threshold, x) count_zeros = x -> count(y -> abs(y) < zero_threshold, x) - zpk_sys = zpk(sys) - phase0 = if sys.Ts == 0.0 - pi/2 * (count_zeros(zpk_sys.matrix[1].z) - count_zeros(zpk_sys.matrix[1].p)) + phase0 = if isa(sys, DelayLtiSystem) + 0.0 else - pi/2 * (count_unit_circle(zpk_sys.matrix[1].z) - count_unit_circle(zpk_sys.matrix[1].p)) + zpk_sys = zpk(sys) + if sys.Ts == 0.0 + pi/2 * (count_zeros(zpk_sys.matrix[1].z) - count_zeros(zpk_sys.matrix[1].p)) + else + pi/2 * (count_unit_circle(zpk_sys.matrix[1].z) - count_unit_circle(zpk_sys.matrix[1].p)) + end end phase = rad2deg.(unwrap!(angle.(resp), 1, init = phase0)) return abs.(resp), phase, w