Improvement on Bode plot phase

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])`
 

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

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
 
 """