use adaptive sampling for the frequency grid in bode and nyquist plots

This should both increase resolution of sharp features and reduce the number of plotted points at the same time

Author: baggepinnen

lib/ControlSystemsBase/src/freqresp.jl

@@ -380,9 +380,14 @@ frequencies `w`. See also [`sigmaplot`](@ref)
 end
 @autovec (1) sigma(sys::LTISystem) = sigma(sys, _default_freq_vector(sys, Val{:sigma}()))
 
-function _default_freq_vector(systems::Vector{<:LTISystem}, plot)
-    min_pt_per_dec = 60
-    min_pt_total = 200
+function _default_freq_vector(systems::Vector{<:LTISystem}, plot; adaptive=false)
+    if adaptive
+        min_pt_per_dec = 100
+        min_pt_total = 1000
+    else
+        min_pt_per_dec = 60
+        min_pt_total = 200
+    end
     bounds = map(sys -> _bounds_and_features(sys, plot)[1], systems)
     w1 = minimum(minimum, bounds)
     w2 = maximum(maximum, bounds)
@@ -394,8 +399,8 @@ function _default_freq_vector(systems::Vector{<:LTISystem}, plot)
     end
     w
 end
-_default_freq_vector(sys::LTISystem, plot) = _default_freq_vector(
-        [sys], plot)
+_default_freq_vector(sys::LTISystem, plot; kwargs...) = _default_freq_vector(
+        [sys], plot; kwargs...)
 
 function _bounds_and_features(sys::LTISystem, plot::Val)
     # Get zeros and poles for each channel

lib/ControlSystemsBase/src/plotting.jl

@@ -227,13 +227,15 @@ Calculate default frequency vector and put system in array of not already array.
 for which `_default_freq_vector` is defined.
 Check that system dimensions are compatible.
 """
-_processfreqplot(plottype, system::LTISystem, args...) =
-    _processfreqplot(plottype, [system], args...)
+_processfreqplot(plottype, system::LTISystem, args...; kwargs...) =
+    _processfreqplot(plottype, [system], args...; kwargs...)
 # Catch when system is not vector, with and without frequency input
 
 # Catch correct form
-function _processfreqplot(plottype, systems::AbstractVector{<:LTISystem},
-            w = _default_freq_vector(systems, plottype))
+_processfreqplot(plottype, systems::AbstractVector{<:LTISystem}; adaptive=false) =
+    _processfreqplot(plottype, systems, _default_freq_vector(systems, plottype; adaptive))
+
+function _processfreqplot(plottype, systems::AbstractVector{<:LTISystem}, w; kwargs...)
 
     if !_same_io_dims(systems...)
         error("All systems must have the same input/output dimensions")
@@ -254,6 +256,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.
+- `adaptive`: If true, an adaptive frequency grid is used in order to keep the number of plotted points low, while resolving features in the frequency response well. If a manually provided frequency vector is used, this may be downsampled before plotting.
 
 `kwargs` is sent as argument to RecipesBase.plot.
 """
@@ -275,8 +278,10 @@ function _get_plotlabel(s, i, j)
     end
 end
 
-@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...)
+_span(vec) = -(reverse(extrema(vec))...)
+
+@recipe function bodeplot(p::Bodeplot; plotphase=true, ylimsphase=(), unwrap=true, hz=false, balance=true, adjust_phase_start=true, adaptive=true)
+    systems, w = _processfreqplot(Val{:bode}(), p.args...; adaptive)
     ws = (hz ? 1/(2π) : 1) .* w
     ny, nu = size(systems[1])
     s2i(i,j) = LinearIndices((nu,(plotphase ? 2 : 1)*ny))[j,i]
@@ -328,7 +333,13 @@ end
                         label --> lab
                     end
                     group     --> group_ind
-                    ws, magdata
+                    if adaptive
+                        lmag = _PlotScale == "dB" ? magdata : log.(magdata)
+                        wsi, _, inds = downsample(ws, lmag, _span(lmag)/300)
+                        wsi, magdata[inds]
+                    else
+                        ws, magdata
+                    end
                 end
                 plotphase || continue
 
@@ -349,9 +360,15 @@ end
                     xguide    --> (hz ? "Frequency [Hz]" : "Frequency [rad/s]")
                     label     --> ""
                     group     --> group_ind
-                    ws, unwrap ? ControlSystemsBase.unwrap(phasedata.*(pi/180)).*(180/pi) : phasedata
-                end
+                    phasedata = unwrap ? ControlSystemsBase.unwrap(phasedata.*(pi/180)).*(180/pi) : phasedata
+                    # NOTE: we should only downsample if the user hasn't provided w themselves
+                    if adaptive
+                        downsample(ws, phasedata, _span(phasedata)/300)[1:2]
+                    else
+                        ws, phasedata
+                    end
 
+                end
             end
         end
     end
@@ -404,8 +421,8 @@ optionally provided.
 `kwargs` is sent as argument to plot.
 """
 nyquistplot
-@recipe function nyquistplot(p::Nyquistplot; Ms_circles=Float64[], Mt_circles=Float64[], unit_circle=false, hz=false, critical_point=-1, balance=true)
-    systems, w = _processfreqplot(Val{:nyquist}(), p.args...)
+@recipe function nyquistplot(p::Nyquistplot; Ms_circles=Float64[], Mt_circles=Float64[], unit_circle=false, hz=false, critical_point=-1, balance=true, adaptive=true)
+    systems, w = _processfreqplot(Val{:nyquist}(), p.args...; adaptive)
     ny, nu = size(systems[1])
     nw = length(w)
     layout --> (ny,nu)
@@ -432,7 +449,14 @@ nyquistplot
                         label --> lab
                     end
                     hover --> [hz ? Printf.@sprintf("f = %.3g", w/2π) : Printf.@sprintf("ω = %.3g", w) for w in w]
-                    (redata, imdata)
+                    if adaptive
+                        indsre = downsample(w, redata, 1/500)[3]
+                        indsim = downsample(w, imdata, 1/500)[3]
+                        inds = sort!(union(indsre, indsim))
+                        redata[inds], imdata[inds]
+                    else
+                        redata, imdata
+                    end
                 end                
 
                 if si == length(systems)
@@ -732,8 +756,8 @@ Plot all the amplitude and phase margins of the system(s) `sys`.
 `kwargs` is sent as argument to RecipesBase.plot.
 """
 marginplot
-@recipe function marginplot(p::Marginplot; plotphase=true, hz=false, balance=true, adjust_phase_start=true)
-    systems, w = _processfreqplot(Val{:bode}(), p.args...)
+@recipe function marginplot(p::Marginplot; plotphase=true, hz=false, balance=true, adjust_phase_start=true, adaptive=true)
+    systems, w = _processfreqplot(Val{:bode}(), p.args...; adaptive)
     ny, nu = size(systems[1])
     s2i(i,j) = LinearIndices((nu,(plotphase ? 2 : 1)*ny))[j,i]
     layout --> ((plotphase ? 2 : 1)*ny, nu)
@@ -790,7 +814,14 @@ marginplot
                     end
                     primary := true
                     seriestype := :bodemag
-                    w, bmag[i, j, :]
+                    m = bmag[i, j, :]
+                    if adaptive
+                        lmag = _PlotScale == "dB" ? m : log.(m)
+                        wsi, _, inds = downsample(w, lmag, _span(lmag)/300)
+                        wsi, m[inds]
+                    else
+                        w, m
+                    end
                 end
 
                 #Plot gain margins
@@ -818,7 +849,11 @@ marginplot
                 @series begin
                     primary := true
                     seriestype := :bodephase
-                    w, phasedata
+                    if adaptive
+                        downsample(w, phasedata, _span(phasedata)/300)[1:2]
+                    else
+                        w, phasedata
+                    end
                 end
                 @series begin
                     primary := false
@@ -917,7 +952,7 @@ Gang-of-Four plot.
 `sigma` determines whether a [`sigmaplot`](@ref) is used instead of a [`bodeplot`](@ref) for MIMO `S` and `T`.
 `kwargs` are sent as argument to RecipesBase.plot.
 """
-function gangoffourplot(P::Union{<:Vector, LTISystem}, C::Vector, args...; minimal=true, Ms_lines = [1.0, 1.25, 1.5], Mt_lines = [], sigma = true,  plotphase=false, kwargs...)    
+function gangoffourplot(P::Union{<:Vector, LTISystem}, C::Vector, args...; minimal=true, Ms_lines = [1.0, 1.25, 1.5], Mt_lines = [], sigma = true,  plotphase=false, adaptive=true, kwargs...)    
     if P isa LTISystem # Don't broadcast over scalar (with size?)
         P = [P]
     end
@@ -927,16 +962,16 @@ function gangoffourplot(P::Union{<:Vector, LTISystem}, C::Vector, args...; minim
 
     gofs = gangoffour.(P,C)
     S,D,N,T = ntuple(i->getindex.(gofs, i), 4)
-    bp = (args...; kwargs...) -> sigma ? sigmaplot(args...; kwargs...) : bodeplot(args...; plotphase=false, kwargs...)
+    bp = (args...; kwargs...) -> sigma ? sigmaplot(args...; kwargs...) : bodeplot(args...; plotphase=false, adaptive, kwargs...)
     f1 = bp(S, args...; show=false, title="S = 1/(1+PC)", kwargs...)
     if !isnothing(Ms_lines) && !isempty(Ms_lines)
         Plots.hline!(Ms_lines', l=(:dash, [:green :orange :red :darkred :purple]), sp=1, primary=false, lab=string.(Ms_lines'), ylims=(1e-2,4))
     else
         Plots.hline!([1.0], l=(:dash, :black), sp=1, ylims=(1e-2,1.8))
     end
-    f2 = bodeplot(D, args...; show=false, title="P/(1+PC)", plotphase=false, kwargs...)
+    f2 = bodeplot(D, args...; show=false, title="P/(1+PC)", plotphase=false, adaptive, kwargs...)
     Plots.hline!(ones(1, ninputs(D[1])*noutputs(D[1])), l=(:black, :dash), primary=false)
-    f3 = bodeplot(N, args...; show=false, title="C/(1+PC)", plotphase=false, kwargs...)
+    f3 = bodeplot(N, args...; show=false, title="C/(1+PC)", plotphase=false, adaptive, kwargs...)
     f4 = bp(T, args...; show=false, title="T = PC/(1+PC)", ylims=(1e-2,4), kwargs...)
     if !isnothing(Mt_lines) && !isempty(Mt_lines)
         Plots.hline!(Mt_lines', l=(:dash, [:green :orange :red :darkred :purple]), primary=false, lab=string.(Mt_lines'), ylims=(1e-2,4))
@@ -989,3 +1024,76 @@ rgaplot
         end
     end
 end
+
+
+## Adaptive sampling
+# Code adapted from https://github.com/iuliancioarca/AdaptiveSampling.jl/blob/master/LICENSE
+
+function downsample(t,y,detail_th)
+    # Compress signal by removing redundant points.
+    # Adjust waveform detail/compression ratio with 'detail_th' (maximum allowed
+    # difference between original and approximated points from the signal)
+    yln           = length(y)
+    idx_l         = 1
+    idx_r         = yln
+    idx_d_max     = 1
+    cond_break    = true
+    d_max         = 0.0
+    M             = zeros(Int,yln) # hash table for relevant indices
+    idx2save      = zeros(Int,yln+2)
+    cnt           = 2
+    idx2save[1:2] = [1,yln]
+    while cond_break
+        # get maximum error(difference) and index, between original chunk of signal
+        # and linear approximation
+        d_max, idx_d_max = get_d_max(idx_l,idx_r,y)
+        # save all indices
+        M[idx_d_max] = idx_r
+        if d_max > detail_th
+            # if computed error is greater than maximum allowed error, save
+            # next point index and call get_d_max(idx_l,idx_r,y) at next
+            # iteration; keep going towards leftmost branches
+            cnt           = cnt + 1
+            idx_r         = idx_d_max
+            idx2save[cnt] = idx_d_max
+        else
+            # if computed error is smaller than maximum allowed error, stop, go
+            # right(to the next waveform segment) and call get_d_max(idx_l,idx_r,y)
+            # at the next iteration
+            idx_l     = idx_r;
+            if idx_l != yln
+                idx_r = M[idx_l]
+            else
+                cond_break = false
+            end
+        end
+    end
+    # sort all indexes corresponding to relevent points and generate resampled
+    # signal
+    idx2save = idx2save[1:cnt]
+    idx2save = sort(idx2save)
+    t_new    = @view t[idx2save]
+    y_new    = @view y[idx2save]
+    return t_new, y_new, idx2save
+end
+function get_d_max(idx_l,idx_r,y)
+    # cut segment to be resampled
+    yp = view(y,idx_l:idx_r)
+    # construct linear approximation
+    dr = LinRange(y[idx_l], y[idx_r], length(yp))
+    # compute distance(error) and get index of maximum error
+    # -> this will be used for further splitting the
+    # signal and will be part of the final resampled signal
+    d_max     = 0.0
+    idx_d_max = 1
+    err_val   = 0.0
+    for i = 1:length(yp)
+        err_val = abs(yp[i] - dr[i])
+        if err_val > d_max
+            d_max     = err_val
+            idx_d_max = i
+        end
+    end
+    idx_d_max = idx_d_max + idx_l - 1
+    return d_max, idx_d_max
+end

lib/ControlSystemsBase/test/test_plots.jl

@@ -55,6 +55,8 @@ end
 
   sys = ssrand(3,3,3)
   sigmaplot(sys, extrema=true)
+  bodeplot(sys, adaptive=false)
+  nyquistplot(sys, adaptive=false)
 
   Gmimo = ssrand(2,2,2,Ts=1)
   @test_nowarn plot(step(Gmimo, 10), plotx=true)