WIP: Adaptive sampling

src/ControlSystems.jl

@@ -145,6 +145,7 @@ include("types/zpk.jl")
 include("types/lqg.jl") # QUESTION: is it really motivated to have an LQG type?
 
 include("utilities.jl")
+include("auto_grid.jl")
 
 include("types/promotion.jl")
 include("types/conversion.jl")

src/auto_grid.jl

@@ -0,0 +1,121 @@
+
+
+"""
+values, grid = auto_grid(func, lims::Tuple, xscale, yscales; start_gridpoints, maxgridpoints, mineps, reltol_dd, abstol_dd)
+
+    Arguments:
+    Compute a `grid` that tries to capture all features of the function `func` in the range `lim=(xmin,xmax)`.
+    `func`: `Function` so that that returns a `Tuple` of values, e.g. `func(x) = (v1,v2,...,vN)` where each `vi` is a scalar or AbstractArray.
+        note that the behavior for non scalar vi might be unintuitive.
+    The algorithm will try to produce a grid so that features are captured in all of the values, but widely varying scaled might cause problems.
+    `xscale`: `Tuple` containing a monotone function and its inverse corresponding to the default axis.
+        Example: If you want to produce a plot with logarithmic x-axis, then you should set `xscale=(log10, exp10)`.
+    `yscales`: `Tuple` containing one monotone function for each of the values `(v1,v2,...,vN)` corresponding to the default axis.
+
+    Kwargs:
+    `start_gridpoints`: The number of initial grid points.
+    `maxgridpoints`: A warning will be thrown if this number of gridpoints are reached.
+    `mineps`: Lower limit on how small steps will be taken (in `xscale`).
+        Example: with `xscale=(log10, exp10)` and `eps=1e-2`then `log10(grid[k+1])-log10(grid[k]) > (1e-2)/2`, i.e `grid[k+1] > 1.012grid[k]`.
+    `reltol_dd = 0.05`, `abstol_dd = 0.01`: The criteria for further refinement is
+        `norm(2*y2-y1-y3) < max(reltol_dd*max(norm(y2-y1), norm(y3-y2)), abstol_dd)`,
+        where `y1,y2,y3` (in yscale) are 3 equally spaced points (in xscale).
+
+    Output: 
+    `values`: `Tuple` with one vector per output value of `func`.
+    `grid`: `Vector` with the grid corresponding to the values, i.e `func(grid[i])[j] = values[j][i]`
+
+    Example: The following code computes a grid and plots the functions: abs(sinc(x)) and cos(x)+1
+        in lin-log and log-log plots respectively.
+
+    func = x -> (abs(sinc(x)),cos(x)+1)
+    lims = (0.1,7.0)
+    xscale = (log10, exp10)
+    yscales = (identity, log10)
+    y, x = auto_grid(func, lims, xscale, yscales, mineps=1e-3)
+
+    plot(x, y[1], m=:o; xscale=:log10, layout=(2,1), yscale=:identity, subplot=1, lab="sinc(x)", size=(800,600))
+    plot!(x, y[2], m=:o, xscale=:log10, subplot=2, yscale=:log10, lab="cos(x)+1", ylims=(1e-5,10))
+
+"""
+function auto_grid(func, lims::Tuple, xscale::Tuple, yscales::Tuple;
+                start_gridpoints=30, maxgridpoints=2000,
+                mineps=(xscale[1](lims[2])-xscale[1](lims[1]))/10000,
+                reltol_dd=0.05, abstol_dd=1e-2)
+
+    # Linearly spaced grid in xscale
+    init_grid = LinRange(xscale[1](lims[1]), xscale[1](lims[2]), start_gridpoints)
+    # Current count of number mindpoints
+    num_gridpoints = start_gridpoints
+
+    # Current left point
+    xleft = init_grid[1]
+    xleft_identity = xscale[2](xleft)
+    leftvalue = func(xleft_identity)
+    # The full set of gridpoints
+    grid = [xleft_identity,]
+    # Tuple with list of all values (Faster than list of tuples + reshaping)
+    values = ntuple(i -> [leftvalue[i],], length(yscales))
+
+    for xright in init_grid[2:end]
+        # Scale back to identity
+        xright_identity = xscale[2](xright)
+        rightvalue = func(xright_identity)
+        # Refine (if nessesary) section (xleft,xright)
+        num_gridpoints = refine_grid!(values, grid, func, xleft, xright, leftvalue, rightvalue, xscale, yscales, maxgridpoints, mineps, num_gridpoints; reltol_dd, abstol_dd)
+        # We are now done with [xleft,xright]
+        # Continue to next segment
+        xleft = xright
+        leftvalue = rightvalue
+    end
+    return values, grid
+end
+
+function push_multiple!(vecs::Tuple, vals::Tuple)
+    push!.(vecs, vals)
+end
+
+# Given range (xleft, xright) potentially add more gridpoints. xleft is assumed to be already added, xright will be added
+function refine_grid!(values, grid, func, xleft, xright, leftvalue, rightvalue, xscale, yscales, maxgridpoints, mineps, num_gridpoints; reltol_dd=0.05, abstol_dd=1e-2)
+    # In scaled grid
+    midpoint = (xleft+xright)/2
+    # Scaled back
+    midpoint_identity = xscale[2](midpoint)
+    midvalue = func(midpoint_identity)
+
+    (num_gridpoints >= maxgridpoints) && @warn "Maximum number of gridpoints reached in refine_grid! at $midpoint_identity, no further refinement will be made. Increase maxgridpoints to get better accuracy." maxlog=1
+    #mineps in scaled version, abs to avoid assuming monotonly increasing scale
+    if (abs(xright - xleft) >= mineps) && (num_gridpoints < maxgridpoints) && !is_almost_linear(xleft, midpoint, xright, leftvalue, midvalue, rightvalue, xscale, yscales; reltol_dd, abstol_dd)
+        num_gridpoints = refine_grid!(values, grid, func, xleft,    midpoint, leftvalue, midvalue,   xscale, yscales, maxgridpoints, mineps, num_gridpoints; reltol_dd, abstol_dd)
+        num_gridpoints = refine_grid!(values, grid, func, midpoint, xright,   midvalue,  rightvalue, xscale, yscales, maxgridpoints, mineps, num_gridpoints; reltol_dd, abstol_dd)
+    else
+        # No more refinement needed, add computed points
+        push!(grid, midpoint_identity)
+        push!(grid, xscale[2](xright))
+        push_multiple!(values, midvalue)
+        push_multiple!(values, rightvalue)
+        num_gridpoints += 2
+    end
+    return num_gridpoints
+end
+
+# TODO We can scale when saving instead of recomputing scaling
+# Svectors should also be faster in general
+function is_almost_linear(xleft, midpoint, xright, leftvalue, midvalue, rightvalue, xscale, yscales; reltol_dd=0.05, abstol_dd=1e-2)
+    # We assume that x2-x1 \approx  x3-x2, so we need to check that y2-y1 approx y3-y2
+
+    # x1 = xscale.(xleft)
+    # x2 = xscale.(midpoint)
+    # x3 = xscale.(xright)
+    y1 = apply_tuple2(yscales, leftvalue)
+    y2 = apply_tuple2(yscales, midvalue)
+    y3 = apply_tuple2(yscales, rightvalue)
+    d1 = (y2-y1)
+    d2 = (y3-y2)
+    # Essentially low second derivative compared to derivative, so that linear approximation is good.
+    # Second argument to avoid too small steps when derivatives are small
+    norm(d1-d2) < max(reltol_dd*max(norm(d1), norm(d2)), abstol_dd)
+end
+
+# Seems to be fast enough
+apply_tuple2(fs::Tuple, x) = [fs[i].(x[i]) for i in 1:length(fs)]

src/delay_systems.jl

@@ -4,18 +4,18 @@ function freqresp(sys::DelayLtiSystem, ω::AbstractVector{T}) where {T <: Real}
 
     P_fr = freqresp(sys.P.P, ω);
 
-    G_fr = zeros(eltype(P_fr), length(ω), ny, nu)
+    G_fr = zeros(eltype(P_fr), ny, nu,  length(ω))
 
     for ω_idx=1:length(ω)
-        P11_fr = P_fr[ω_idx, 1:ny, 1:nu]
-        P12_fr = P_fr[ω_idx, 1:ny, nu+1:end]
-        P21_fr = P_fr[ω_idx, ny+1:end, 1:nu]
-        P22_fr = P_fr[ω_idx, ny+1:end, nu+1:end]
+        P11_fr = P_fr[1:ny, 1:nu, ω_idx]
+        P12_fr = P_fr[1:ny, nu+1:end, ω_idx]
+        P21_fr = P_fr[ny+1:end, 1:nu, ω_idx]
+        P22_fr = P_fr[ny+1:end, nu+1:end, ω_idx]
 
         delay_matrix_inv_fr = Diagonal(exp.(im*ω[ω_idx]*sys.Tau)) # Frequency response of the diagonal matrix with delays
         # Inverse of the delay matrix, so there should not be any minus signs in the exponents
 
-        G_fr[ω_idx,:,:] .= P11_fr + P12_fr/(delay_matrix_inv_fr - P22_fr)*P21_fr # The matrix is invertible (?!)
+        G_fr[:,:,ω_idx] .= P11_fr + P12_fr/(delay_matrix_inv_fr - P22_fr)*P21_fr # The matrix is invertible (?!)
     end
 
     return G_fr

src/freqresp.jl

@@ -1,22 +1,45 @@
+function eval_frequency(sys::LTISystem, w::Real)
+    if iscontinuous(sys)
+        return evalfr(sys,im*w)
+    else
+        return evalfr(sys, exp(w*im*sys.Ts))
+    end
+end
+
 """sys_fr = freqresp(sys, w)
 
 Evaluate the frequency response of a linear system
 
 `w -> C*((iw*im -A)^-1)*B + D`
 
-of system `sys` over the frequency vector `w`."""
-@autovec () function freqresp(sys::LTISystem, w_vec::AbstractVector{<:Real})
-    # Create imaginary freq vector s
-    if iscontinuous(sys)
-        s_vec = im*w_vec
-    else
-        s_vec = exp.(w_vec*(im*sys.Ts))
-    end
+of system `sys` over the frequency vector `w`.
+
+`sys_fr` has size `(ny, nu, length(w))`
+"""
+@autovec (1) function freqresp(sys::LTISystem, w_vec::AbstractVector{<:Real})
     #if isa(sys, StateSpace)
     #    sys = _preprocess_for_freqresp(sys)
     #end
-    ny,nu = noutputs(sys), ninputs(sys)
-    [evalfr(sys[i,j], s)[] for s in s_vec, i in 1:ny, j in 1:nu]
+    mapfoldl(w -> eval_frequency(sys, w), (x,y) -> cat(x,y,dims=3), w_vec)
+end
+
+# TODO Most of this logic should be moved to the respective options, e.g. bode
+# TODO Less copying of code
+"""sys_fr, w = freqresp(sys::LTISystem, lims::Tuple)
+
+Evaluate the frequency response of a linear system
+
+`w -> C*((iw*im -A)^-1)*B + D`
+
+of system `sys` for frequencies `w` between `lims[1]` and `lims[2]`.
+
+`sys_fr` has size `(ny, nu, length(w))`
+"""
+@autovec (1) function freqresp(sys::LTISystem, lims::Tuple)
+    # TODO What is the usecase here? Defaulting to identity for now
+    f = (w) -> (eval_frequency(sys, w),)
+    ys, grid = auto_grid(f, lims, (identity, identity), (identity,))
+    return cat(ys[1]..., dims=3), grid
 end
 
 # Implements algorithm found in:
@@ -100,50 +123,87 @@ end
 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)`""" 
+`mag` and `phase` has size `(ny, nu, length(w))`""" 
 @autovec (1, 2) function bode(sys::LTISystem, w::AbstractVector)
     resp = freqresp(sys, w)
     return abs.(resp), rad2deg.(unwrap!(angle.(resp),1)), w
 end
-@autovec (1, 2) bode(sys::LTISystem) = bode(sys, _default_freq_vector(sys, Val{:bode}()))
+@autovec (1, 2) bode(sys::LTISystem; kwargs...) = bode(sys, _default_freq_lims(sys, Val{:bode}()); kwargs...)
+
+@autovec (1, 2) function bode(sys::LTISystem, lims::Tuple; kwargs...)
+    f = (w) -> begin
+        fr = eval_frequency(sys, w)
+        (abs.(fr), angle.(fr))
+    end
+    ys, grid = auto_grid(f, lims, (log10, exp10), (log10, identity); kwargs...)
+    angles = cat(ys[2]...,dims=3)
+    unwrap!(angles,3)
+    angles .= rad2deg.(angles)
+    cat(ys[1]...,dims=3), angles, grid
+end
 
 """`re, im, w = nyquist(sys[, w])`
 
 Compute the real and imaginary parts of the frequency response of system `sys`
 at frequencies `w`
 
-`re` and `im` has size `(length(w), ny, nu)`""" 
+`re` and `im` has size `(ny, nu, length(w))`""" 
 @autovec (1, 2) function nyquist(sys::LTISystem, w::AbstractVector)
     resp = freqresp(sys, w)
     return real(resp), imag(resp), w
 end
-@autovec (1, 2) nyquist(sys::LTISystem) = nyquist(sys, _default_freq_vector(sys, Val{:nyquist}()))
+@autovec (1, 2) function nyquist(sys::LTISystem, lims::Tuple; kwargs...)
+    # TODO check if better to only consider fr
+    f = (w) -> begin
+        fr = eval_frequency(sys, w)
+        (fr, real.(fr), imag.(fr))
+    end
+    ys, grid = auto_grid(f, lims, (log10, exp10), (identity,identity,identity); kwargs...)
+    return cat(ys[2]...,dims=3), cat(ys[3]...,dims=3), grid
+end
+@autovec (1, 2) nyquist(sys::LTISystem; kwargs...) = nyquist(sys, _default_freq_lims(sys, Val{:nyquist}()); kwargs...)
 
 """`sv, w = sigma(sys[, w])`
 
 Compute the singular values `sv` of the frequency response of system `sys` at
 frequencies `w`
 
-`sv` has size `(length(w), max(ny, nu))`""" 
+`sv` has size `(max(ny, nu), length(w))`""" 
 @autovec (1) function sigma(sys::LTISystem, w::AbstractVector)
     resp = freqresp(sys, w)
-    sv = dropdims(mapslices(svdvals, resp, dims=(2,3)),dims=3)
+    sv = dropdims(mapslices(svdvals, resp, dims=(1,2)),dims=2)
     return sv, w
 end
-@autovec (1) sigma(sys::LTISystem) = sigma(sys, _default_freq_vector(sys, Val{:sigma}()))
+# TODO: Not tested, probably broadcast problem on svdvals in auto_grid
+@autovec (1) function sigma(sys::LTISystem, lims::Tuple; kwargs...)
+    f = (w) -> begin
+        fr = eval_frequency(sys, w)
+        (svdvals(fr),)
+    end
+    ys, grid = auto_grid(f, lims, (log10, exp10), (log10,); kwargs...)
+    return cat(ys[1]...,dims=2), grid
+end
+@autovec (1) sigma(sys::LTISystem; kwargs...) = sigma(sys, _default_freq_lims(sys, Val{:sigma}()); kwargs...)
 
-function _default_freq_vector(systems::Vector{<:LTISystem}, plot)
-    min_pt_per_dec = 60
-    min_pt_total = 200
+function _default_freq_lims(systems, plot)
     bounds = map(sys -> _bounds_and_features(sys, plot)[1], systems)
     w1 = minimum(minimum.(bounds))
     w2 = maximum(maximum.(bounds))
+    return exp10(w1), exp10(w2)
+end
 
+function _default_freq_vector(systems::Vector{<:LTISystem}, plot)
+    min_pt_per_dec = 60
+    min_pt_total = 200
+    w1, w2 = _default_freq_lims(systems, plot)
     nw = round(Int, max(min_pt_total, min_pt_per_dec*(w2 - w1)))
     return exp10.(range(w1, stop=w2, length=nw))
 end
+
 _default_freq_vector(sys::LTISystem, plot) = _default_freq_vector(
         [sys], plot)
+_default_freq_lims(sys::LTISystem, plot) = _default_freq_lims(
+    [sys], plot)
 
 
 function _bounds_and_features(sys::LTISystem, plot::Val)

src/plotting.jl

@@ -280,12 +280,12 @@ bodeplot
         for j=1:nu
             for i=1:ny
                 group_ind += 1
-                magdata = vec(mag[:, i, j])
+                magdata = vec(mag[i, j, :])
                 if all(magdata .== -Inf)
                     # 0 system, don't plot anything
                     continue
                 end
-                phasedata = vec(phase[:, i, j])
+                phasedata = vec(phase[i, j, :])
                 @series begin
                     yscale    --> _PlotScaleFunc
                     xscale    --> :log10
@@ -388,8 +388,8 @@ nyquistplot
         re_resp, im_resp = nyquist(s, w)[1:2]
         for j=1:nu
             for i=1:ny
-                redata = re_resp[:, i, j]
-                imdata = im_resp[:, i, j]
+                redata = re_resp[i, j, :]
+                imdata = im_resp[i, j, :]
                 @series begin
                     ylims --> (min(max(-20,minimum(imdata)),-1), max(min(20,maximum(imdata)),1))
                     xlims --> (min(max(-20,minimum(redata)),-1), max(min(20,maximum(redata)),1))
@@ -637,7 +637,7 @@ sigmaplot
                 xscale --> :log10
                 yscale --> _PlotScaleFunc
                 seriescolor --> si
-                w, sv[:, i]
+                w, sv[i, :]
             end
         end
     end