diff --git a/src/freqresp.jl b/src/freqresp.jl index 21bf882fa..f5f676261 100644 --- a/src/freqresp.jl +++ b/src/freqresp.jl @@ -102,11 +102,27 @@ 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) - return abs.(resp), rad2deg.(unwrap!(angle.(resp),1)), 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) < unit_circle_threshold, x) + count_zeros = x -> count(y -> abs(y) < zero_threshold, x) + + phase0 = if isa(sys, DelayLtiSystem) + 0.0 + else + 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 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])` 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 diff --git a/src/utilities.jl b/src/utilities.jl index e2ea1b95c..3c352401c 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 + eltype(M)(init) + 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 """