From fcbf840cfb000f31155f046257561f0a890c3fc0 Mon Sep 17 00:00:00 2001 From: olof3 Date: Tue, 22 Jan 2019 10:03:23 +0100 Subject: [PATCH 01/30] WIP: Delayed LTI systems. --- example/delayed_lti_system.jl | 19 ++++ src/ControlSystems.jl | 9 ++ src/delay_systems.jl | 26 +++++ src/types/DelayLtiSystem.jl | 72 +++++++++++++ src/types/PartionedStateSpace.jl | 160 ++++++++++++++++++++++++++++ test/test_delayed_systems.jl | 44 ++++++++ test/test_partitioned_statespace.jl | 41 +++++++ 7 files changed, 371 insertions(+) create mode 100644 example/delayed_lti_system.jl create mode 100644 src/delay_systems.jl create mode 100644 src/types/DelayLtiSystem.jl create mode 100644 src/types/PartionedStateSpace.jl create mode 100644 test/test_delayed_systems.jl create mode 100644 test/test_partitioned_statespace.jl diff --git a/example/delayed_lti_system.jl b/example/delayed_lti_system.jl new file mode 100644 index 000000000..df6a68296 --- /dev/null +++ b/example/delayed_lti_system.jl @@ -0,0 +1,19 @@ +using Plots + +# Frequency domain analysis of a simple system with time delays + +s = tf("s") + +P = delay(0.2) * DelayLtiSystem(ss(1/(s+1))) + +K = 3; Ti = 0.3; +C = DelayLtiSystem(ss(K*(1 + 1/s))) + +ω = exp10.(LinRange(-2,2,500)) + +L_fr = freqresp(C*P, ω)[:] +plot(real(L_fr), imag(L_fr), xlim=[-2,1], ylim=[-2,2], title="Nyquist curve") + +G_yd = feedback(P, C) +plot(ω, abs.(freqresp(G_yd, ω)[:]), xscale=:log, yscale=:log, + title="Transfer function from load disturbances to output.") diff --git a/src/ControlSystems.jl b/src/ControlSystems.jl index 69f07b137..e45cf022d 100644 --- a/src/ControlSystems.jl +++ b/src/ControlSystems.jl @@ -3,6 +3,7 @@ module ControlSystems export LTISystem, StateSpace, TransferFunction, + DelayLtiSystem, ss, tf, zpk, @@ -65,6 +66,8 @@ export LTISystem, bode, nyquist, sigma, + # delay systems + delay, # utilities num, #Deprecated den, #Deprecated @@ -78,6 +81,7 @@ export LTISystem, using Polynomials, OrdinaryDiffEq, Plots, LaTeXStrings, LinearAlgebra export Plots import Base: +, -, *, /, (==), (!=), isapprox, convert, promote_op +import Base: getproperty import LinearAlgebra: BlasFloat export lyap # Make sure LinearAlgebra.lyap is available import Printf, Colors @@ -99,6 +103,9 @@ include("types/SisoTfTypes/conversion.jl") include("types/StateSpace.jl") +include("types/PartionedStateSpace.jl") +include("types/DelayLtiSystem.jl") + # Convenience constructors include("types/tf.jl") include("types/zpk.jl") @@ -126,6 +133,8 @@ include("synthesis.jl") include("simulators.jl") include("pid_design.jl") +include("delay_systems.jl") + include("plotting.jl") @deprecate num numvec diff --git a/src/delay_systems.jl b/src/delay_systems.jl new file mode 100644 index 000000000..aa3c0ae34 --- /dev/null +++ b/src/delay_systems.jl @@ -0,0 +1,26 @@ +function freqresp(sys::DelayLtiSystem, ω::AbstractVector{T}) where {T <: Real} + ny = noutputs(sys) + nu = ninputs(sys) + + P_fr = ControlSystems.freqresp(sys.P, ω); + + println(ω) + + # FIXME: Different dimensions compared to standard freqresp + 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] + + # FIXME: when there is no delays... + + delay_vect_fr = Base.exp.(im*sys.Tau*ω[ω_idx]) # Frequency response of the block diagonal matrix + + G_fr[:,:,ω_idx] .= P11_fr + P12_fr/(Diagonal(delay_vect_fr) - P22_fr)*P21_fr # The matrix is invertible (?!) + end + + return G_fr +end diff --git a/src/types/DelayLtiSystem.jl b/src/types/DelayLtiSystem.jl new file mode 100644 index 000000000..101d91b5d --- /dev/null +++ b/src/types/DelayLtiSystem.jl @@ -0,0 +1,72 @@ +struct DelayLtiSystem{T} <: LTISystem + P::StateSpace{T,Matrix{T}} + Tau::Vector{Float64} # The length of the vector tau implicitly defines the partitionging of P + + # function DelayLtiSystem(P::StateSpace{T, MT}, Tau::Vector{Float64}) + # if ControlSystems.noutputs(P) < length(Tau) || + # ControlSystems.noutputs(P) < length(Tau) + # error("Length of time-vector is too long given the size of the partitioned system P.") + # end + # new{T}(P, Tau) + # end +end + +DelayLtiSystem(sys::StateSpace) = DelayLtiSystem(sys, Float64[]) +#Base.convert(::Type{S}, ) where {S<:DelayLtiSystem} = # Need to handle numerical arguments + +Base.promote_rule(::Type{<:StateSpace}, ::Type{S}) where {S<:DelayLtiSystem} = S + +ninputs(sys::DelayLtiSystem) = size(sys.P)[2] - length(sys.Tau) +noutputs(sys::DelayLtiSystem) = size(sys.P)[1] - length(sys.Tau) + + +function +(sys1::DelayLtiSystem, sys2::DelayLtiSystem) + s1 = PartionedStateSpace(sys1.P, ninputs(sys1), noutputs(sys1)) + s2 = PartionedStateSpace(sys2.P, ninputs(sys2), noutputs(sys2)) + + s_new = s1 + s2 + Tau_new = [sys1.Tau; sys2.Tau] + + DelayLtiSystem(s_new.P, Tau_new) +end + + +function *(sys1::DelayLtiSystem, sys2::DelayLtiSystem) + s1 = PartionedStateSpace(sys1.P, ninputs(sys1), noutputs(sys1)) + s2 = PartionedStateSpace(sys2.P, ninputs(sys2), noutputs(sys2)) + + s_new = s1 * s2 + Tau_new = [sys1.Tau; sys2.Tau] + + DelayLtiSystem(s_new.P, Tau_new) +end + + +function feedback(sys1::DelayLtiSystem, sys2::DelayLtiSystem) + s1 = PartionedStateSpace(sys1.P, ninputs(sys1), noutputs(sys1)) + s2 = PartionedStateSpace(sys2.P, ninputs(sys2), noutputs(sys2)) + + s_new = feedback(s1, s2) + Tau_new = [sys1.Tau; sys2.Tau] + + DelayLtiSystem(s_new.P, Tau_new) +end + +function delay(tau::Real, T::Type{<:Number}=Float64) + return DelayLtiSystem(ControlSystems.ss([zero(T) one(T); one(T) zero(T)]), [float(tau)]) +end + +# function exp(G::TransferFunction) +# if (size(G.matrix) != [1, 1]) || ~isone(G.matrix[1].den) || length(G.matrix[1].num) >= 2 +# error("exp only accepts TransferFunction arguemns of the form (a*s + b)") +# end +# +# a = G.matrix[1].num[1] +# b = G.matrix[1].num[0] +# +# if a > 0 +# error("Delay needs to be causal.") +# end +# +# return exp(b) * delay(a) +# end diff --git a/src/types/PartionedStateSpace.jl b/src/types/PartionedStateSpace.jl new file mode 100644 index 000000000..8908ae358 --- /dev/null +++ b/src/types/PartionedStateSpace.jl @@ -0,0 +1,160 @@ +""" +A StateSpace model with a partioning imposed according to + +A | B1 B2 +——— ———————— +C1 | D11 D12 +C2 | D21 D22 + +It corresponds to partioned input and output signals +u = [u1 u2]^T +y = [y1 y2]^T + +""" +struct PartionedStateSpace{S} + P::S + nu1::Int + ny1::Int +end + + +function getproperty(sys::PartionedStateSpace, d::Symbol) + P = getfield(sys, :P) + nu1 = getfield(sys, :nu1) + ny1 = getfield(sys, :ny1) + + if d == :P + return P + elseif d == :nu1 + return nu1 + elseif d == :ny1 + return ny1 + elseif d == :A + return P.A + elseif d == :B1 + return P.B[:, 1:nu1] + elseif d == :B2 + return P.B[:, nu1+1:end] + elseif d == :C1 + return P.C[1:ny1, :] + elseif d == :C2 + return P.C[ny1+1:end, :] + elseif d == :D11 + return P.D[1:ny1, 1:nu1] + elseif d == :D12 + return P.D[1:ny1, nu1+1:end] + elseif d == :D21 + return P.D[ny1+1:end, 1:nu1] + elseif d == :D22 + return P.D[ny1+1:end, nu1+1:end] + else + return getfield(P, d) + end +end + + +# There should already exist a function like this somewhare? +function blkdiag(A1::Matrix{T1}, A2::Matrix{T2}) where {T1<:Number, T2<:Number} + T = promote_type(T1, T2) + + dims1 = size(A1) + dims2 = size(A2) + + A_new = zeros(Float64, dims1 .+ dims2) + A_new[1:dims1[1], 1:dims1[2]] = A1 + A_new[dims1[1]+1:end, dims1[2]+1:end] = A2 + + return A_new +end + + +function +(s1::PartionedStateSpace, s2::PartionedStateSpace) + A = blkdiag(s1.A, s2.A) + + B = [[s1.B1; s2.B1] blkdiag(s1.B2, s2.B2)] + + C = [[s1.C1 s2.C1]; + blkdiag(s1.C2, s2.C2)] + + D = [(s1.D11 + s2.D11) s1.D12 s2.D12; + [s1.D21; s2.D21] blkdiag(s1.D22, s2.D22)] + + P = StateSpace(A, B, C, D, 0) # How to handle discrete? + PartionedStateSpace(P, s1.nu1 + s2.nu1, s1.ny1 + s2.ny1) +end + + + + + +""" + Series connection of to partioned StateSpace systems. +""" +function *(s1::PartionedStateSpace, s2::PartionedStateSpace) + println("Hej") + A = [s1.A s1.B1*s2.C1; + zeros(size(s2.A,1),size(s1.A,2)) s2.A] + + B = [s1.B1*s2.D11 s1.B2 s1.B1*s2.D12; + s2.B1 zeros(size(s2.B2,1),size(s1.B2,2)) s2.B2] + + C = [s1.C1 s1.D11*s2.C1; + s1.C2 s1.D21*s2.C1; + zeros(size(s2.C2,1),size(s1.C2,2)) s2.C2] + + D = [s1.D11*s2.D11 s1.D12 s1.D11*s2.D12; + s1.D21*s2.D11 s1.D22 s1.D21*s2.D12; + s2.D21 zeros(size(s2.D22,1),size(s1.D22,2)) s2.D22 ] + + P = StateSpace(A, B, C, D, 0) + PartionedStateSpace(P, s2.nu1, s1.ny1) +end + + + +# Algebraic loops?! + +function feedback(s1::PartionedStateSpace, s2::PartionedStateSpace) + X_11 = [-s2.D11*s1.C1 -s2.C1] + X_21 = [s1.C1 -s1.D11*s2.C1] + + # For the case of two outputs + # X_12 = [I -s2.D11 -s2.D11*s1.D12 -s2.D12] + # X_22 = [-s1.D11 I -s1.D12 s1.D11*s2.D12] + X_12 = [I -s2.D11*s1.D12 -s2.D12] + X_22 = [-s1.D11 -s1.D12 s1.D11*s2.D12] + + + + A = [s1.B1 * X_11 ; s2.B1 * X_21] + blkdiag(s1.A, s2.A) + + B = [s1.B1 * X_12 ; s2.B1 * X_22] + tmp = blkdiag(s1.B2, s2.B2) + B[:, end-size(tmp,2)+1:end] .+= tmp + + C = [s1.D11 * X_11 ; + s1.D21 * X_11 ; + s2.D21 * X_21 ] + [s1.C1 zeros(size(s1.C1,1),size(s2.C1,2)); blkdiag(s1.C2, s2.C2)] + + D = [s1.D11 * X_12 ; + s1.D21 * X_12 ; + s2.D21 * X_22 ] + tmp = [s1.D12 zeros(size(s1.D12,1),size(s2.D12,2)); blkdiag(s1.D22, s2.D22)] + D[:, end-size(tmp,2)+1:end] .+= tmp + + # in case it is desired to consider both outputs + # C = [s1.D11 * X_11 ; + # s2.D11 * X_21 ; + # s1.D21 * X_11 ; + # s2.D21 * X_21 ] + [blkdiag(s1.C1, s2.C1); blkdiag(s1.C2, s2.C2)] + # + # D = [s1.D11 * X_12 ; + # s2.D11 * X_22 ; + # s1.D21 * X_12 ; + # s2.D21 * X_22 ] + #tmp = [blkdiag(s1.D12, s2.D12); blkdiag(s1.D22, s2.D22)] + #D[:, end-size(tmp,2)+1:end] .+= tmp + + P = StateSpace(A, B, C, D, 0) + PartionedStateSpace(P, s2.nu1, s1.ny1) +end diff --git a/test/test_delayed_systems.jl b/test/test_delayed_systems.jl new file mode 100644 index 000000000..c30a9a818 --- /dev/null +++ b/test/test_delayed_systems.jl @@ -0,0 +1,44 @@ +ω = 0.0:8 + +freqresp(delay(1), ω)[:] ≈ exp.(-im*ω) +freqresp(delay(2.5), ω)[:] ≈ exp.(-2.5*im*ω) + +P1 = DelayLtiSystem(ss(-1.0, 1, 1, 0)) +P1_fr ≈ 1 ./ (im*ω .+ 1) + +P2 = DelayLtiSystem(ss(-2.0, -1, 1, 1)) # (s+1)/(s+2) +P2_fr = (im*ω .+ 1) ./ (im*ω .+ 2) + + +freqresp(P1 * delay(1), ω)[:] ≈ P1_fr .* exp.(-im*ω) +freqresp(delay(1) * P1, ω)[:] ≈ P1_fr .* exp.(-im*ω) + +freqresp(P2 * delay(1), ω)[:] ≈ P2_fr .* exp.(-im*ω) +freqresp(delay(1) * P2, ω)[:] ≈ P2_fr .* exp.(-im*ω) + + +# feedback +freqresp(feedback(P1, delay(1)), ω)[:] ≈ P1_fr ./ (1 .+ exp.(-im*ω) .* P1_fr) +freqresp(feedback(delay(1), P1), ω)[:] ≈ exp.(-im*ω) ./ (1 .+ exp.(-im*ω) .* P1_fr) + +G_fr = freqresp(feedback(P1*delay(1), DelayLtiSystem(ss(1.0))), ω)[:] +G_fr ≈ P1_fr .* exp.(-im*ω) ./ (1 .+ exp.(-im*ω) .* P1_fr) + + +# Test DelayLtiSystem objects without delays +freqresp(feedback(P1, P1), ω)[:] ≈ P1_fr ./ (1 .+ P1_fr .* P1_fr) + + +# FIXME: Automatic conversion to DelayLtiSystem + + +nvert(DelayLtiSystem, P1) + +promote_rule(typeof(P1), typeof(P2)) +x1, x2 = promote(P1, P2) + + +# What ordering for freqresp ?! +freqresp(P.P, ω) + +G_fr = freqresp(G, ω)[:] diff --git a/test/test_partitioned_statespace.jl b/test/test_partitioned_statespace.jl new file mode 100644 index 000000000..cee4a5034 --- /dev/null +++ b/test/test_partitioned_statespace.jl @@ -0,0 +1,41 @@ +using ControlSystems +using LinearAlgebra + +## +# system of the form +# 1 | 2 3 +# ——————— +# 4 | 5 6 +# 7 | 8 9 +sys1 = ControlSystems.PartionedStateSpace(ss(1.0, [2.0 3.0], [4.0; 7.0], [5.0 6.0; 8.0 9.0]), 1, 1) + +matrix(x::Number) = fill(x, 1, 1) + +sys1.A == matrix(1.0) +sys1.B1 == matrix(2.0) +sys1.B2 == matrix(3.0) +sys1.C1 == matrix(4.0) +sys1.C2 == matrix(7.0) +sys1.D11 == matrix(5.0) +sys1.D12 == matrix(6.0) +sys1.D21 == matrix(8.0) +sys1.D22 == matrix(9.0) + + +## +sys1 = ControlSystems.PartionedStateSpace(ss(fill(1.0, 2, 2), fill(2.0, 2, 5), fill(3.0, 7, 2), fill(4.0, 7, 5)), 2, 3) + +sys1.A == fill(1.0, 2, 2) +sys1.B1 == fill(2.0, 2, 2) +sys1.B2 == fill(2.0, 2, 3) +sys1.C1 == fill(3.0, 3, 2) +sys1.C2 == fill(3.0, 4, 2) +sys1.D11 == fill(4.0, 3, 2) +sys1.D12 == fill(4.0, 3, 3) +sys1.D21 == fill(4.0, 4, 2) +sys1.D22 == fill(4.0, 4, 3) + + +# Simple test +(sys1 + sys1).P[1, 1] == (sys1.P[1,1] + sys1.P[1,1]) +(sys1 * sys1).P[1, 1] == (sys1.P[1,1] * sys1.P[1,1]) From 3eff27ce40689ec89bebe0edfecbe51811cd0d9c Mon Sep 17 00:00:00 2001 From: olof3 Date: Wed, 23 Jan 2019 11:28:35 +0100 Subject: [PATCH 02/30] Delayed LTI updates. --- example/delayed_lti_system.jl | 2 +- example/delayed_lti_timeresp.jl | 41 ++++++++++++++++++++++++++++++ src/delay_systems.jl | 11 +++----- src/types/DelayLtiSystem.jl | 20 +++++++++++---- src/types/Lti.jl | 3 +++ src/types/PartionedStateSpace.jl | 9 +++---- test/test_delayed_systems.jl | 43 +++++++++++++++----------------- 7 files changed, 87 insertions(+), 42 deletions(-) create mode 100644 example/delayed_lti_timeresp.jl diff --git a/example/delayed_lti_system.jl b/example/delayed_lti_system.jl index df6a68296..0587d01a4 100644 --- a/example/delayed_lti_system.jl +++ b/example/delayed_lti_system.jl @@ -4,7 +4,7 @@ using Plots s = tf("s") -P = delay(0.2) * DelayLtiSystem(ss(1/(s+1))) +P = delay(0.2) * ss(1/(s+1)) K = 3; Ti = 0.3; C = DelayLtiSystem(ss(K*(1 + 1/s))) diff --git a/example/delayed_lti_timeresp.jl b/example/delayed_lti_timeresp.jl new file mode 100644 index 000000000..eea308ccc --- /dev/null +++ b/example/delayed_lti_timeresp.jl @@ -0,0 +1,41 @@ +function simulate(sys::DelayLtiSystem, tspan; u=[], x0=Float64[], alg=MethodOfSteps(Tsit5()), kwargs...) + ControlSystems.PartionedStateSpace(sys.P, 1, 1) + + nu = ControlSystems.ninputs(sys) + nx = ControlSystems.nstates(sys) + + u = (u == []) ? t -> fill(0.0, nu) : u + x0 = (x0 == []) ? fill(0.0, nx) : x0 + + # FIXME: Only works for one single time delay + # FIXME: Should error for non-zero D22 terms + dde = function (dx, x, h, p, t) + d_delayed = P.C2*h(p,t-sys.Tau[1]) + P.D21*u(t-sys.Tau[1])# + P.D22*(t-Tau[1] + dx .= P.A*x + P.B2*d_delayed + P.B1*u(t) + end + + h_initial = (p, t) -> zeros(N) + + + saved_values_y = SavedValues(Float64, Vector{Float64}) + cb = SavingCallback((x,t,integrator)->(P.C1*x + P.D11*u(t)), saved_values_y) + + prob = DDEProblem(dde, x0, h_initial, tspan, constant_lags=sys.Tau) + + sol = solve(prob, alg; saveat=0:0.02:tspan[2], callback=cb, kwargs...) + + t, x = sol.t, sol.u + + + t, x +end + +sys = feedback(1.0, ss(-1, 1, 1, 0) * delay(1.0)) + +@time t, x = simulate(sys, (0.0,4.0); u=t->[t>0 ? 1.0 : 0.0], saveat=0:0.02:5) + +x1 = [x[k][1] for k=1:length(x)] +del = [zeros(50); x1[1:end-50]] # FIXME: Not the real delayed signal... + +# The following does not work in general... just this specific problem instance +plot(t, ones(size(t)) .- x1) diff --git a/src/delay_systems.jl b/src/delay_systems.jl index aa3c0ae34..ad0e1b21d 100644 --- a/src/delay_systems.jl +++ b/src/delay_systems.jl @@ -4,10 +4,7 @@ function freqresp(sys::DelayLtiSystem, ω::AbstractVector{T}) where {T <: Real} P_fr = ControlSystems.freqresp(sys.P, ω); - println(ω) - - # FIXME: Different dimensions compared to standard freqresp - G_fr = zeros(eltype(P_fr), ny, nu, length(ω)) + G_fr = zeros(eltype(P_fr), length(ω), ny, nu) for ω_idx=1:length(ω) P11_fr = P_fr[ω_idx, 1:ny, 1:nu] @@ -15,11 +12,9 @@ function freqresp(sys::DelayLtiSystem, ω::AbstractVector{T}) where {T <: Real} P21_fr = P_fr[ω_idx, ny+1:end, 1:nu] P22_fr = P_fr[ω_idx, ny+1:end, nu+1:end] - # FIXME: when there is no delays... - - delay_vect_fr = Base.exp.(im*sys.Tau*ω[ω_idx]) # Frequency response of the block diagonal matrix + delay_matrix_fr = Diagonal(exp.(im*sys.Tau*ω[ω_idx])) # Frequency response of the diagonal matrix with delays - G_fr[:,:,ω_idx] .= P11_fr + P12_fr/(Diagonal(delay_vect_fr) - P22_fr)*P21_fr # The matrix is invertible (?!) + G_fr[ω_idx,:,:] .= P11_fr + P12_fr/(delay_matrix_fr - P22_fr)*P21_fr # The matrix is invertible (?!) end return G_fr diff --git a/src/types/DelayLtiSystem.jl b/src/types/DelayLtiSystem.jl index 101d91b5d..b6e6c4910 100644 --- a/src/types/DelayLtiSystem.jl +++ b/src/types/DelayLtiSystem.jl @@ -11,14 +11,24 @@ struct DelayLtiSystem{T} <: LTISystem # end end -DelayLtiSystem(sys::StateSpace) = DelayLtiSystem(sys, Float64[]) -#Base.convert(::Type{S}, ) where {S<:DelayLtiSystem} = # Need to handle numerical arguments +DelayLtiSystem(sys::StateSpace{T,MT}) where {T, MT} = DelayLtiSystem{T}(sys, Float64[]) +DelayLtiSystem{T}(sys::StateSpace) where T = DelayLtiSystem{T}(sys, Float64[]) -Base.promote_rule(::Type{<:StateSpace}, ::Type{S}) where {S<:DelayLtiSystem} = S -ninputs(sys::DelayLtiSystem) = size(sys.P)[2] - length(sys.Tau) -noutputs(sys::DelayLtiSystem) = size(sys.P)[1] - length(sys.Tau) +# TODO: Think through these promotions and conversions +Base.promote_rule(::Type{<:StateSpace{T1}}, ::Type{DelayLtiSystem{T2}}) where {T1<:Number,T2<:Number} = DelayLtiSystem{promote_type(T1,T2)} +Base.promote_rule(::Type{AbstractMatrix{T1}}, ::Type{DelayLtiSystem{T2}}) where {T1<:Number,T2<:Number} = DelayLtiSystem{promote_type(T1,T2)} +Base.promote_rule(::Type{T1}, ::Type{DelayLtiSystem{T2}}) where {T1<:Number,T2<:Number} = DelayLtiSystem{promote_type(T1,T2)} +#Base.promote_rule(::Type{<:UniformScaling}, ::Type{S}) where {S<:DelayLtiSystem} = DelayLtiSystem{T} +Base.convert(::Type{DelayLtiSystem{T}}, sys::StateSpace) where T = DelayLtiSystem{T}(sys) +Base.convert(::Type{DelayLtiSystem{T1}}, d::T2) where {T1,T2 <: Number} = DelayLtiSystem{T1}(ss(d)) +#Base.convert(::Type{DelayLtiSystem{T}}, sys::DelayLtiSystem) where T = DelayLtiSystem{T}(StateSpace{T,Matrix{T}}(sys)) + + +ninputs(sys::DelayLtiSystem) = ninputs(sys.P) - length(sys.Tau) +noutputs(sys::DelayLtiSystem) = noutputs(sys.P) - length(sys.Tau) +nstates(sys::DelayLtiSystem) = nstates(sys.P) function +(sys1::DelayLtiSystem, sys2::DelayLtiSystem) s1 = PartionedStateSpace(sys1.P, ninputs(sys1), noutputs(sys1)) diff --git a/src/types/Lti.jl b/src/types/Lti.jl index 4cb8bbd56..5fa9d197c 100644 --- a/src/types/Lti.jl +++ b/src/types/Lti.jl @@ -4,6 +4,9 @@ abstract type LTISystem <: AbstractSystem end *(sys1::LTISystem, sys2::LTISystem) = *(promote(sys1, sys2)...) /(sys1::LTISystem, sys2::LTISystem) = /(promote(sys1, sys2)...) +feedback(sys1::Union{LTISystem,Number,AbstractMatrix{<:Number}}, + sys2::Union{LTISystem,Number,AbstractMatrix{<:Number}}) = feedback(promote(sys1, sys2)...) + """`issiso(sys)` diff --git a/src/types/PartionedStateSpace.jl b/src/types/PartionedStateSpace.jl index 8908ae358..1901f4db6 100644 --- a/src/types/PartionedStateSpace.jl +++ b/src/types/PartionedStateSpace.jl @@ -88,10 +88,9 @@ end """ - Series connection of to partioned StateSpace systems. + Series connection of partioned StateSpace systems. """ function *(s1::PartionedStateSpace, s2::PartionedStateSpace) - println("Hej") A = [s1.A s1.B1*s2.C1; zeros(size(s2.A,1),size(s1.A,2)) s2.A] @@ -112,7 +111,7 @@ end -# Algebraic loops?! +# QUESTION: Should perhaps algebraic loops?! Perhaps issue warning if P1(∞)*P2(∞) > 1 function feedback(s1::PartionedStateSpace, s2::PartionedStateSpace) X_11 = [-s2.D11*s1.C1 -s2.C1] @@ -120,9 +119,9 @@ function feedback(s1::PartionedStateSpace, s2::PartionedStateSpace) # For the case of two outputs # X_12 = [I -s2.D11 -s2.D11*s1.D12 -s2.D12] - # X_22 = [-s1.D11 I -s1.D12 s1.D11*s2.D12] + # X_22 = [s1.D11 I -s1.D12 s1.D11*s2.D12] X_12 = [I -s2.D11*s1.D12 -s2.D12] - X_22 = [-s1.D11 -s1.D12 s1.D11*s2.D12] + X_22 = [s1.D11 -s1.D12 s1.D11*s2.D12] diff --git a/test/test_delayed_systems.jl b/test/test_delayed_systems.jl index c30a9a818..a48b6fc96 100644 --- a/test/test_delayed_systems.jl +++ b/test/test_delayed_systems.jl @@ -1,15 +1,24 @@ ω = 0.0:8 +typeof(promote(delay(0.2), ss(1))[1]) == DelayLtiSystem{Float64} +typeof(promote(delay(0.2), ss(1.0 + im))[1]) == DelayLtiSystem{Float64} + freqresp(delay(1), ω)[:] ≈ exp.(-im*ω) freqresp(delay(2.5), ω)[:] ≈ exp.(-2.5*im*ω) P1 = DelayLtiSystem(ss(-1.0, 1, 1, 0)) -P1_fr ≈ 1 ./ (im*ω .+ 1) +P1_fr = 1 ./ (im*ω .+ 1) P2 = DelayLtiSystem(ss(-2.0, -1, 1, 1)) # (s+1)/(s+2) P2_fr = (im*ω .+ 1) ./ (im*ω .+ 2) +# Addition +freqresp(P1 + delay(1), ω)[:] ≈ P1_fr .+ exp.(-im*ω) +#FIXME: the following gives a crash.. freqresp(P1 - delay(1), ω)[:] ≈ P1_fr .+ exp.(-im*ω) + + +# Multiplication freqresp(P1 * delay(1), ω)[:] ≈ P1_fr .* exp.(-im*ω) freqresp(delay(1) * P1, ω)[:] ≈ P1_fr .* exp.(-im*ω) @@ -17,28 +26,16 @@ freqresp(P2 * delay(1), ω)[:] ≈ P2_fr .* exp.(-im*ω) freqresp(delay(1) * P2, ω)[:] ≈ P2_fr .* exp.(-im*ω) -# feedback -freqresp(feedback(P1, delay(1)), ω)[:] ≈ P1_fr ./ (1 .+ exp.(-im*ω) .* P1_fr) -freqresp(feedback(delay(1), P1), ω)[:] ≈ exp.(-im*ω) ./ (1 .+ exp.(-im*ω) .* P1_fr) - -G_fr = freqresp(feedback(P1*delay(1), DelayLtiSystem(ss(1.0))), ω)[:] -G_fr ≈ P1_fr .* exp.(-im*ω) ./ (1 .+ exp.(-im*ω) .* P1_fr) - - -# Test DelayLtiSystem objects without delays +# Feedback +# The first tests don't include delays, but the linear system is of DelayLtiForm type +# (very simple system so easy to troubleshoot) +freqresp(feedback(1.0, P1), ω)[:] ≈ 1 ./ (1 .+ P1_fr) +freqresp(feedback(P1, 1.0), ω)[:] ≈ P1_fr ./ (1 .+ P1_fr) freqresp(feedback(P1, P1), ω)[:] ≈ P1_fr ./ (1 .+ P1_fr .* P1_fr) +freqresp(feedback(P1, delay(1)), ω)[:] ≈ P1_fr ./ (1 .+ exp.(-im*ω) .* P1_fr) +freqresp(feedback(delay(1), P1), ω)[:] ≈ exp.(-im*ω) ./ (1 .+ exp.(-im*ω) .* P1_fr) #FIXME: Answer is Inf, but should give error.. +freqresp(feedback(P1*delay(1), 1.0), ω)[:] ≈ P1_fr .* exp.(-im*ω) ./ (1 .+ exp.(-im*ω) .* P1_fr) +freqresp(feedback(1.0, P1*delay(1)), ω)[:] ≈ 1 ./ (1 .+ exp.(-im*ω) .* P1_fr) -# FIXME: Automatic conversion to DelayLtiSystem - - -nvert(DelayLtiSystem, P1) - -promote_rule(typeof(P1), typeof(P2)) -x1, x2 = promote(P1, P2) - - -# What ordering for freqresp ?! -freqresp(P.P, ω) - -G_fr = freqresp(G, ω)[:] +#FIXME: A lot more tests, including MIMO systems in particular From 4f33725455d46ea2b062eb6f77c9221c416bd915 Mon Sep 17 00:00:00 2001 From: olof3 Date: Wed, 23 Jan 2019 17:53:05 +0100 Subject: [PATCH 03/30] Updates. --- example/delayed_lti_timeresp.jl | 31 +++++----------------- src/ControlSystems.jl | 3 ++- src/delay_systems.jl | 37 +++++++++++++++++++++++++- src/freqresp.jl | 3 ++- src/types/DelayLtiSystem.jl | 46 +++++++++++++++++++-------------- src/types/StateSpace.jl | 2 ++ test/test_delayed_systems.jl | 5 ++-- 7 files changed, 78 insertions(+), 49 deletions(-) diff --git a/example/delayed_lti_timeresp.jl b/example/delayed_lti_timeresp.jl index eea308ccc..9797441c7 100644 --- a/example/delayed_lti_timeresp.jl +++ b/example/delayed_lti_timeresp.jl @@ -1,40 +1,23 @@ -function simulate(sys::DelayLtiSystem, tspan; u=[], x0=Float64[], alg=MethodOfSteps(Tsit5()), kwargs...) - ControlSystems.PartionedStateSpace(sys.P, 1, 1) +using DifferentialEquations - nu = ControlSystems.ninputs(sys) - nx = ControlSystems.nstates(sys) - u = (u == []) ? t -> fill(0.0, nu) : u - x0 = (x0 == []) ? fill(0.0, nx) : x0 - # FIXME: Only works for one single time delay - # FIXME: Should error for non-zero D22 terms - dde = function (dx, x, h, p, t) - d_delayed = P.C2*h(p,t-sys.Tau[1]) + P.D21*u(t-sys.Tau[1])# + P.D22*(t-Tau[1] - dx .= P.A*x + P.B2*d_delayed + P.B1*u(t) - end - h_initial = (p, t) -> zeros(N) - saved_values_y = SavedValues(Float64, Vector{Float64}) - cb = SavingCallback((x,t,integrator)->(P.C1*x + P.D11*u(t)), saved_values_y) +#sys_d = c2d(sys.P) - prob = DDEProblem(dde, x0, h_initial, tspan, constant_lags=sys.Tau) +#function sim_discr(sys::DelayLtiSystem) - sol = solve(prob, alg; saveat=0:0.02:tspan[2], callback=cb, kwargs...) +sys = feedback(2.0, ss(-1.0, 2, 1, 0) * delay(3.0)) - t, x = sol.t, sol.u +@time t, x, saved_y = ControlSystems.simulate2(sys, (0.0,8.0); u=t->[t>0 ? 1.0 : 0.0], saveat=0:0.02:5) - t, x -end - -sys = feedback(1.0, ss(-1, 1, 1, 0) * delay(1.0)) +saved_y +x1 = [x[k][1] for k=1:length(x)] -@time t, x = simulate(sys, (0.0,4.0); u=t->[t>0 ? 1.0 : 0.0], saveat=0:0.02:5) -x1 = [x[k][1] for k=1:length(x)] del = [zeros(50); x1[1:end-50]] # FIXME: Not the real delayed signal... # The following does not work in general... just this specific problem instance diff --git a/src/ControlSystems.jl b/src/ControlSystems.jl index e45cf022d..94760ef4b 100644 --- a/src/ControlSystems.jl +++ b/src/ControlSystems.jl @@ -78,7 +78,8 @@ export LTISystem, # QUESTION: are these used? LaTeXStrings, Requires, IterTools -using Polynomials, OrdinaryDiffEq, Plots, LaTeXStrings, LinearAlgebra +using Polynomials, Plots, LaTeXStrings, LinearAlgebra +using DifferentialEquations, OrdinaryDiffEq, DelayDiffEq export Plots import Base: +, -, *, /, (==), (!=), isapprox, convert, promote_op import Base: getproperty diff --git a/src/delay_systems.jl b/src/delay_systems.jl index ad0e1b21d..07c455faa 100644 --- a/src/delay_systems.jl +++ b/src/delay_systems.jl @@ -2,7 +2,7 @@ function freqresp(sys::DelayLtiSystem, ω::AbstractVector{T}) where {T <: Real} ny = noutputs(sys) nu = ninputs(sys) - P_fr = ControlSystems.freqresp(sys.P, ω); + P_fr = ControlSystems.freqresp(sys.P.P, ω); G_fr = zeros(eltype(P_fr), length(ω), ny, nu) @@ -19,3 +19,38 @@ function freqresp(sys::DelayLtiSystem, ω::AbstractVector{T}) where {T <: Real} return G_fr end + + + +function simulate5(sys::DelayLtiSystem, tspan; u=[], x0=Float64[], alg=MethodOfSteps(Tsit5()), kwargs...) + P = sys.P + + nu = ControlSystems.ninputs(sys) + nx = ControlSystems.nstates(sys) + + u = (u == []) ? t -> fill(0.0, nu) : u + x0 = (x0 == []) ? fill(0.0, nx) : x0 + + # FIXME: Only works for one single time delay + # FIXME: Should error for non-zero D22 terms + dde = function (dx, x, h, p, t) + d_delayed = P.C2*h(p,t-sys.Tau[1]) + P.D21*u(t-sys.Tau[1])# + P.D22*(t-Tau[1] + dx .= P.A*x + P.B2*d_delayed + P.B1*u(t) + end + + h_initial = (p, t) -> zeros(nx) + + + # Saves y (excluding the d(t-τ) contribution) and d + saved_values_y = SavedValues(Float64, Tuple{Vector{Float64}, Vector{Float64}}) + cb = SavingCallback((x,t,integrator) -> (P.C1*x + P.D11*u(t), P.C2*x + P.D21*u(t)), saved_values_y, saveat=0:0.02:tspan[2]) +#P.C1*x + P.D11*u(t) + P.D12*h(t) + prob = DDEProblem(dde, x0, h_initial, tspan, constant_lags=sys.Tau) + + sol = solve(prob, alg, callback=cb; saveat=0:0.02:tspan[2], kwargs...) + + t, x = sol.t, sol.u + + + t, x, saved_values_y +end diff --git a/src/freqresp.jl b/src/freqresp.jl index b8b3a8f99..6a3dc2241 100644 --- a/src/freqresp.jl +++ b/src/freqresp.jl @@ -34,7 +34,8 @@ function _preprocess_for_freqresp(sys::StateSpace) F = hessenberg(A) T = F.Q P = C*T - Q = T\B # TODO Type stability? + Q = T\B # TODO Type stability? # T is unitary, so mutliplication with T' should do the trick + # FIXME; No performance improvement from Hessienberg structure, also weired renaming of matrices StateSpace(F.H, Q, P, D, sys.Ts) end diff --git a/src/types/DelayLtiSystem.jl b/src/types/DelayLtiSystem.jl index b6e6c4910..a39c0bdf2 100644 --- a/src/types/DelayLtiSystem.jl +++ b/src/types/DelayLtiSystem.jl @@ -1,5 +1,5 @@ struct DelayLtiSystem{T} <: LTISystem - P::StateSpace{T,Matrix{T}} + P::PartionedStateSpace{StateSpace{T,Matrix{T}}} Tau::Vector{Float64} # The length of the vector tau implicitly defines the partitionging of P # function DelayLtiSystem(P::StateSpace{T, MT}, Tau::Vector{Float64}) @@ -11,6 +11,21 @@ struct DelayLtiSystem{T} <: LTISystem # end end +# QUESTION: would psys be a good standard variable name for a PartionedStateSpace +# and perhaps dsys for a delayed system, (ambigous with discrete system though) +function DelayLtiSystem{T}(sys::StateSpace, Tau::Vector{Float64}) where T + nu = ninputs(sys) - length(Tau) + ny = noutputs(sys) - length(Tau) + + if nu < 0 || ny < 0 + error("Time vector is too long.") + end + + psys = PartionedStateSpace(sys, ny, nu) + DelayLtiSystem{T}(psys, Tau) +end + +DelayLtiSystem(sys::StateSpace{T,MT}, Tau::Vector{Float64}) where {T, MT} = DelayLtiSystem{T}(sys, Tau) DelayLtiSystem(sys::StateSpace{T,MT}) where {T, MT} = DelayLtiSystem{T}(sys, Float64[]) DelayLtiSystem{T}(sys::StateSpace) where T = DelayLtiSystem{T}(sys, Float64[]) @@ -23,43 +38,34 @@ Base.promote_rule(::Type{T1}, ::Type{DelayLtiSystem{T2}}) where {T1<:Number,T2<: Base.convert(::Type{DelayLtiSystem{T}}, sys::StateSpace) where T = DelayLtiSystem{T}(sys) Base.convert(::Type{DelayLtiSystem{T1}}, d::T2) where {T1,T2 <: Number} = DelayLtiSystem{T1}(ss(d)) -#Base.convert(::Type{DelayLtiSystem{T}}, sys::DelayLtiSystem) where T = DelayLtiSystem{T}(StateSpace{T,Matrix{T}}(sys)) +Base.convert(::Type{DelayLtiSystem{T}}, sys::DelayLtiSystem) where T = DelayLtiSystem{T}(StateSpace{T,Matrix{T}}(sys.P.P), sys.Tau) -ninputs(sys::DelayLtiSystem) = ninputs(sys.P) - length(sys.Tau) -noutputs(sys::DelayLtiSystem) = noutputs(sys.P) - length(sys.Tau) -nstates(sys::DelayLtiSystem) = nstates(sys.P) +ninputs(sys::DelayLtiSystem) = size(sys.P.P, 2) - length(sys.Tau) +noutputs(sys::DelayLtiSystem) = size(sys.P.P, 1) - length(sys.Tau) +nstates(sys::DelayLtiSystem) = nstates(sys.P.P) function +(sys1::DelayLtiSystem, sys2::DelayLtiSystem) - s1 = PartionedStateSpace(sys1.P, ninputs(sys1), noutputs(sys1)) - s2 = PartionedStateSpace(sys2.P, ninputs(sys2), noutputs(sys2)) - - s_new = s1 + s2 + psys_new = sys1.P + sys2.P Tau_new = [sys1.Tau; sys2.Tau] - DelayLtiSystem(s_new.P, Tau_new) + DelayLtiSystem(psys_new.P, Tau_new) end function *(sys1::DelayLtiSystem, sys2::DelayLtiSystem) - s1 = PartionedStateSpace(sys1.P, ninputs(sys1), noutputs(sys1)) - s2 = PartionedStateSpace(sys2.P, ninputs(sys2), noutputs(sys2)) - - s_new = s1 * s2 + psys_new = sys1.P * sys2.P Tau_new = [sys1.Tau; sys2.Tau] - DelayLtiSystem(s_new.P, Tau_new) + DelayLtiSystem(psys_new.P, Tau_new) end function feedback(sys1::DelayLtiSystem, sys2::DelayLtiSystem) - s1 = PartionedStateSpace(sys1.P, ninputs(sys1), noutputs(sys1)) - s2 = PartionedStateSpace(sys2.P, ninputs(sys2), noutputs(sys2)) - - s_new = feedback(s1, s2) + psys_new = feedback(sys1.P, sys2.P) Tau_new = [sys1.Tau; sys2.Tau] - DelayLtiSystem(s_new.P, Tau_new) + DelayLtiSystem(psys_new.P, Tau_new) end function delay(tau::Real, T::Type{<:Number}=Float64) diff --git a/src/types/StateSpace.jl b/src/types/StateSpace.jl index 73dc1ea80..8d7ecdbc7 100644 --- a/src/types/StateSpace.jl +++ b/src/types/StateSpace.jl @@ -51,6 +51,8 @@ function StateSpace(A::AbstractArray, B::AbstractArray, C::AbstractArray, D::Abs to_matrix(T, D), Float64(Ts)) end +StateSpace{T,MT}(sys::StateSpace) where {T,MT} = StateSpace{T,MT}(MT(sys.A), MT(sys.B), MT(sys.C), MT(sys.D), sys.Ts) + # Getter functions get_A(sys::StateSpace) = sys.A get_B(sys::StateSpace) = sys.B diff --git a/test/test_delayed_systems.jl b/test/test_delayed_systems.jl index a48b6fc96..743d9039a 100644 --- a/test/test_delayed_systems.jl +++ b/test/test_delayed_systems.jl @@ -1,7 +1,8 @@ ω = 0.0:8 -typeof(promote(delay(0.2), ss(1))[1]) == DelayLtiSystem{Float64} -typeof(promote(delay(0.2), ss(1.0 + im))[1]) == DelayLtiSystem{Float64} +# broken: typeof(promote(delay(0.2), ss(1))[1]) == DelayLtiSystem{Float64} + +typeof(promote(delay(0.2), ss(1.0 + im))[1]) == DelayLtiSystem{Complex{Float64}} freqresp(delay(1), ω)[:] ≈ exp.(-im*ω) freqresp(delay(2.5), ω)[:] ≈ exp.(-2.5*im*ω) From 3d774b7727d70c653afd4cede00b8aeb24910526 Mon Sep 17 00:00:00 2001 From: olof3 Date: Wed, 23 Jan 2019 18:48:53 +0100 Subject: [PATCH 04/30] Updates to delayed LTI. --- example/delayed_lti_timeresp.jl | 28 ++++++++++++++-------------- src/delay_systems.jl | 14 ++++++++++---- 2 files changed, 24 insertions(+), 18 deletions(-) diff --git a/example/delayed_lti_timeresp.jl b/example/delayed_lti_timeresp.jl index 9797441c7..58817b128 100644 --- a/example/delayed_lti_timeresp.jl +++ b/example/delayed_lti_timeresp.jl @@ -1,24 +1,24 @@ using DifferentialEquations - - - - +plotly() #sys_d = c2d(sys.P) -#function sim_discr(sys::DelayLtiSystem) - -sys = feedback(2.0, ss(-1.0, 2, 1, 0) * delay(3.0)) - -@time t, x, saved_y = ControlSystems.simulate2(sys, (0.0,8.0); u=t->[t>0 ? 1.0 : 0.0], saveat=0:0.02:5) +sys = feedback(1.0, ss(-1.0, 2, 1, 0) * (delay(2.0) + delay(3.0))) +@time t, x, saved_y = ControlSystems.simulate5(sys, (0.0,8.0); u=t->[t>0 ? 1.0 : 0.0], saveat=0:0.02:8) -saved_y -x1 = [x[k][1] for k=1:length(x)] +y = hcat([saved_y.saveval[k][1] for k=1:length(t)]...) +d = hcat([saved_y.saveval[k][2] for k=1:length(t)]...) +for k=1:length(sys.Tau) + N_del = Integer(50*sys.Tau[k]) + dk = [zeros(N_del); d[k, 1:end-N_del]] -del = [zeros(50); x1[1:end-50]] # FIXME: Not the real delayed signal... + for j=1:length(t) + y[:, j] .+= sys.P.D12[:, k] * dk[j] + end +end -# The following does not work in general... just this specific problem instance -plot(t, ones(size(t)) .- x1) +plot(t, y') +gui() diff --git a/src/delay_systems.jl b/src/delay_systems.jl index 07c455faa..4b2ddbdce 100644 --- a/src/delay_systems.jl +++ b/src/delay_systems.jl @@ -25,17 +25,23 @@ end function simulate5(sys::DelayLtiSystem, tspan; u=[], x0=Float64[], alg=MethodOfSteps(Tsit5()), kwargs...) P = sys.P + if ~iszero(P.D22) + error("non-zero D22-matrix block is not supported") # Due to limitations in differential equations + end + nu = ControlSystems.ninputs(sys) nx = ControlSystems.nstates(sys) u = (u == []) ? t -> fill(0.0, nu) : u x0 = (x0 == []) ? fill(0.0, nx) : x0 - # FIXME: Only works for one single time delay - # FIXME: Should error for non-zero D22 terms dde = function (dx, x, h, p, t) - d_delayed = P.C2*h(p,t-sys.Tau[1]) + P.D21*u(t-sys.Tau[1])# + P.D22*(t-Tau[1] - dx .= P.A*x + P.B2*d_delayed + P.B1*u(t) + dx .= P.A*x + P.B1*u(t) + for k=1:length(sys.Tau) # Add each of the delayed signals + dk_delayed = dot(P.C2[k,:], h(p,t-sys.Tau[k])) + dot(P.D21[k,:], u(t-sys.Tau[k]))# + P.D22*(t-Tau[1] + dx .+= P.B2[:, k] * dk_delayed + end + #d_delayed = zeros(size(P.C2,1)) end h_initial = (p, t) -> zeros(nx) From 9af3b838439b5ff3a4b6d2fdb90de877f3f43b67 Mon Sep 17 00:00:00 2001 From: olof3 Date: Thu, 24 Jan 2019 12:20:42 +0100 Subject: [PATCH 05/30] Updates delayed LTI systems. --- REQUIRE | 2 + example/delayed_lti_timeresp.jl | 4 -- src/delay_systems.jl | 5 ++- src/types/DelayLtiSystem.jl | 7 +++ src/types/PartionedStateSpace.jl | 8 ++-- test/test_delayed_systems.jl | 77 +++++++++++++++++++++++++------- 6 files changed, 76 insertions(+), 27 deletions(-) diff --git a/REQUIRE b/REQUIRE index b2fd64d17..1deda6f8d 100644 --- a/REQUIRE +++ b/REQUIRE @@ -2,6 +2,8 @@ julia 0.7 Plots Polynomials LaTeXStrings +DifferentialEquations +DelayDiffEq OrdinaryDiffEq IterTools Colors diff --git a/example/delayed_lti_timeresp.jl b/example/delayed_lti_timeresp.jl index 58817b128..aedf74a34 100644 --- a/example/delayed_lti_timeresp.jl +++ b/example/delayed_lti_timeresp.jl @@ -1,9 +1,5 @@ -using DifferentialEquations - plotly() -#sys_d = c2d(sys.P) - sys = feedback(1.0, ss(-1.0, 2, 1, 0) * (delay(2.0) + delay(3.0))) @time t, x, saved_y = ControlSystems.simulate5(sys, (0.0,8.0); u=t->[t>0 ? 1.0 : 0.0], saveat=0:0.02:8) diff --git a/src/delay_systems.jl b/src/delay_systems.jl index 4b2ddbdce..8651ceeb8 100644 --- a/src/delay_systems.jl +++ b/src/delay_systems.jl @@ -12,9 +12,10 @@ function freqresp(sys::DelayLtiSystem, ω::AbstractVector{T}) where {T <: Real} P21_fr = P_fr[ω_idx, ny+1:end, 1:nu] P22_fr = P_fr[ω_idx, ny+1:end, nu+1:end] - delay_matrix_fr = Diagonal(exp.(im*sys.Tau*ω[ω_idx])) # Frequency response of the diagonal matrix with delays + delay_matrix_inv_fr = Diagonal(exp.(im*sys.Tau*ω[ω_idx])) # 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_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 diff --git a/src/types/DelayLtiSystem.jl b/src/types/DelayLtiSystem.jl index a39c0bdf2..24e7c9713 100644 --- a/src/types/DelayLtiSystem.jl +++ b/src/types/DelayLtiSystem.jl @@ -41,6 +41,13 @@ Base.convert(::Type{DelayLtiSystem{T1}}, d::T2) where {T1,T2 <: Number} = DelayL Base.convert(::Type{DelayLtiSystem{T}}, sys::DelayLtiSystem) where T = DelayLtiSystem{T}(StateSpace{T,Matrix{T}}(sys.P.P), sys.Tau) +function *(sys::DelayLtiSystem, n::Number) + new_C = [sys.P.C1*n; sys.P.C2] + new_D = [sys.P.D11*n sys.P.D12; sys.P.D21*n sys.P.D22] + return DelayLtiSystem(StateSpace(sys.P.A, sys.P.B, new_C, new_D, sys.P.Ts), sys.Tau) +end +*(n::Number, sys::DelayLtiSystem) = *(sys, n) + ninputs(sys::DelayLtiSystem) = size(sys.P.P, 2) - length(sys.Tau) noutputs(sys::DelayLtiSystem) = size(sys.P.P, 1) - length(sys.Tau) nstates(sys::DelayLtiSystem) = nstates(sys.P.P) diff --git a/src/types/PartionedStateSpace.jl b/src/types/PartionedStateSpace.jl index 1901f4db6..cffe82df3 100644 --- a/src/types/PartionedStateSpace.jl +++ b/src/types/PartionedStateSpace.jl @@ -114,14 +114,14 @@ end # QUESTION: Should perhaps algebraic loops?! Perhaps issue warning if P1(∞)*P2(∞) > 1 function feedback(s1::PartionedStateSpace, s2::PartionedStateSpace) - X_11 = [-s2.D11*s1.C1 -s2.C1] - X_21 = [s1.C1 -s1.D11*s2.C1] + X_11 = (I + s2.D11*s1.D11)\[-s2.D11*s1.C1 -s2.C1] + X_21 = (I + s1.D11*s2.D11)\[s1.C1 -s1.D11*s2.C1] # For the case of two outputs # X_12 = [I -s2.D11 -s2.D11*s1.D12 -s2.D12] # X_22 = [s1.D11 I -s1.D12 s1.D11*s2.D12] - X_12 = [I -s2.D11*s1.D12 -s2.D12] - X_22 = [s1.D11 -s1.D12 s1.D11*s2.D12] + X_12 = (I + s2.D11*s1.D11)\[I -s2.D11*s1.D12 -s2.D12] + X_22 = (I + s1.D11*s2.D11)\[s1.D11 s1.D12 -s1.D11*s2.D12] diff --git a/test/test_delayed_systems.jl b/test/test_delayed_systems.jl index 743d9039a..710e6c589 100644 --- a/test/test_delayed_systems.jl +++ b/test/test_delayed_systems.jl @@ -2,41 +2,84 @@ # broken: typeof(promote(delay(0.2), ss(1))[1]) == DelayLtiSystem{Float64} -typeof(promote(delay(0.2), ss(1.0 + im))[1]) == DelayLtiSystem{Complex{Float64}} +@test typeof(promote(delay(0.2), ss(1.0 + im))[1]) == DelayLtiSystem{Complex{Float64}} -freqresp(delay(1), ω)[:] ≈ exp.(-im*ω) -freqresp(delay(2.5), ω)[:] ≈ exp.(-2.5*im*ω) +@test freqresp(delay(1), ω)[:] ≈ exp.(-im*ω) rtol=1e-15 +@test freqresp(delay(2.5), ω)[:] ≈ exp.(-2.5im*ω) rtol=1e-15 +@test freqresp(3.5*delay(2.5), ω)[:] ≈ 3.5*exp.(-2.5im*ω) rtol=1e-15 +@test freqresp(delay(2.5)*1.5, ω)[:] ≈ exp.(-2.5im*ω)*1.5 rtol=1e-15 +# Stritcly proper system P1 = DelayLtiSystem(ss(-1.0, 1, 1, 0)) P1_fr = 1 ./ (im*ω .+ 1) +@test freqresp(P1, ω)[:] == P1_fr +# Not stritcly proper system P2 = DelayLtiSystem(ss(-2.0, -1, 1, 1)) # (s+1)/(s+2) P2_fr = (im*ω .+ 1) ./ (im*ω .+ 2) - +@test freqresp(P2, ω)[:] ≈ P2_fr rtol=1e-15 # Addition -freqresp(P1 + delay(1), ω)[:] ≈ P1_fr .+ exp.(-im*ω) +@test_broken freqresp(1 + delay(1), ω)[:] ≈ 1 .+ exp.(-im*ω) # FIXME; Add suitable conversion from Int64 to DelayLtiSystem +@test freqresp(P1 + delay(1), ω)[:] ≈ P1_fr .+ exp.(-im*ω) + #FIXME: the following gives a crash.. freqresp(P1 - delay(1), ω)[:] ≈ P1_fr .+ exp.(-im*ω) # Multiplication -freqresp(P1 * delay(1), ω)[:] ≈ P1_fr .* exp.(-im*ω) -freqresp(delay(1) * P1, ω)[:] ≈ P1_fr .* exp.(-im*ω) +@test freqresp(P1 * delay(1), ω)[:] ≈ P1_fr .* exp.(-im*ω) rtol=1e-15 +@test freqresp(delay(1) * P1, ω)[:] ≈ P1_fr .* exp.(-im*ω) rtol=1e-15 -freqresp(P2 * delay(1), ω)[:] ≈ P2_fr .* exp.(-im*ω) -freqresp(delay(1) * P2, ω)[:] ≈ P2_fr .* exp.(-im*ω) +@test freqresp(P2 * delay(1), ω)[:] ≈ P2_fr .* exp.(-im*ω) rtol=1e-15 +@test freqresp(delay(1) * P2, ω)[:] ≈ P2_fr .* exp.(-im*ω) rtol=1e-15 # Feedback # The first tests don't include delays, but the linear system is of DelayLtiForm type # (very simple system so easy to troubleshoot) -freqresp(feedback(1.0, P1), ω)[:] ≈ 1 ./ (1 .+ P1_fr) -freqresp(feedback(P1, 1.0), ω)[:] ≈ P1_fr ./ (1 .+ P1_fr) -freqresp(feedback(P1, P1), ω)[:] ≈ P1_fr ./ (1 .+ P1_fr .* P1_fr) - -freqresp(feedback(P1, delay(1)), ω)[:] ≈ P1_fr ./ (1 .+ exp.(-im*ω) .* P1_fr) -freqresp(feedback(delay(1), P1), ω)[:] ≈ exp.(-im*ω) ./ (1 .+ exp.(-im*ω) .* P1_fr) #FIXME: Answer is Inf, but should give error.. -freqresp(feedback(P1*delay(1), 1.0), ω)[:] ≈ P1_fr .* exp.(-im*ω) ./ (1 .+ exp.(-im*ω) .* P1_fr) -freqresp(feedback(1.0, P1*delay(1)), ω)[:] ≈ 1 ./ (1 .+ exp.(-im*ω) .* P1_fr) +@test freqresp(feedback(1.0, P1), ω)[:] ≈ 1 ./ (1 .+ P1_fr) rtol=1e-15 +@test freqresp(feedback(P1, 1.0), ω)[:] ≈ P1_fr ./ (1 .+ P1_fr) rtol=1e-15 +@test freqresp(feedback(P1, P1), ω)[:] ≈ P1_fr ./ (1 .+ P1_fr .* P1_fr) rtol=1e-15 + +@test freqresp(feedback(1.0, DelayLtiSystem(ss(0.5))), [0])[:] == [2/3] +@test freqresp(feedback(1.0, P2), ω)[:] ≈ 1 ./ (1 .+ P2_fr) + + +freqresp(feedback(1.0, ss(0.5) + 0.5*delay(2) + 0.5*delay(3)), ω)[:] +freqresp(feedback(1.0, G), ω)[:] + + +@test freqresp(feedback(0.5, delay(2.0)), ω) ≈ 0.5 ./ (1 .+ 0.5*exp.(-2im*ω)) +@test freqresp(feedback(delay(2.0), 0.5), ω) ≈ exp.(-2im*ω) ./ (1 .+ 0.5*exp.(-2im*ω)) + +@test freqresp(feedback(P1, delay(1)), ω)[:] ≈ P1_fr ./ (1 .+ exp.(-im*ω) .* P1_fr) rtol=1e-15 +@test freqresp(feedback(delay(1), P1), ω)[:] ≈ exp.(-im*ω) ./ (1 .+ exp.(-im*ω) .* P1_fr) #FIXME: Answer is Inf, but should give error.. rtol=1e-15 +@test freqresp(feedback(P1*delay(1), 1.0), ω)[:] ≈ P1_fr .* exp.(-im*ω) ./ (1 .+ exp.(-im*ω) .* P1_fr) rtol=1e-15 +@test freqresp(feedback(1.0, P1*delay(1)), ω)[:] ≈ 1 ./ (1 .+ exp.(-im*ω) .* P1_fr) rtol=1e-15 + +@test freqresp(feedback(1.0, P2*0.5*(ss(1.0) + delay(2))), ω)[:] ≈ 1 ./(1 .+ P2_fr .* 0.5.*(1 .+ exp.(-2*im*ω))) + + +@test_broken freqresp(1.0 + delay(2), ω)[:] # TODO: Add addition for DelayLtiSystem. how to do this in a convenient ammner? + +G = ss(0.5) + 0.5*delay(2)# + 0.5*delay(3) +G_fr = 0.5 .+ 0.5*exp.(-2*im*ω)# + 0.5*exp.(-3*im*ω) +@test freqresp(G, ω)[:] ≈ G_fr rtol=1e-15 + +@test freqresp(feedback(1.0, P1*G), ω)[:] ≈ 1 ./(1 .+ P1_fr .* G_fr) rtol=1e-15 +@test freqresp(feedback(P1, G), ω)[:] ≈ P1_fr ./(1 .+ P1_fr .* G_fr) rtol=1e-15 + +@test freqresp(feedback(P2, G), ω)[:] ≈ P2_fr ./(1 .+ P2_fr .* G_fr) rtol=1e-15 + +sys = feedback(1.0, P1 * (delay(2) + delay(3))) +expected_sys_fr = 1.0 ./ (P1_fr .* (exp.(-2*im*ω) + exp.(-3*im*ω)) .+ 1) +@test freqresp(sys, ω)[:] ≈ expected_sys_fr rtol=1e-14 + +## Multiple Delays +G = ss(1.0) + delay(2) + delay(3) +G_fr = 1 .+ exp.(-2*im*ω) .+ exp.(-3*im*ω) +@test freqresp(G, ω)[:] ≈ G_fr rtol=1e-15 +@test freqresp(feedback(1.0, G), ω)[:] ≈ 1 ./(1 .+ G_fr) # Somewhat pathological system though + #FIXME: A lot more tests, including MIMO systems in particular From a51cc548907fa77c2b3cc8886e73de8aac93f565 Mon Sep 17 00:00:00 2001 From: olof3 Date: Thu, 24 Jan 2019 13:50:52 +0100 Subject: [PATCH 06/30] Updates delayed LTI systems. --- example/delayed_lti_timeresp.jl | 18 +++--------- src/delay_systems.jl | 44 ++++++++++++++++++----------- src/types/PartionedStateSpace.jl | 4 +-- test/runtests.jl | 4 ++- test/test_delayed_systems.jl | 23 +++++++-------- test/test_partitioned_statespace.jl | 31 ++++++++++---------- 6 files changed, 62 insertions(+), 62 deletions(-) diff --git a/example/delayed_lti_timeresp.jl b/example/delayed_lti_timeresp.jl index aedf74a34..a7e44e591 100644 --- a/example/delayed_lti_timeresp.jl +++ b/example/delayed_lti_timeresp.jl @@ -1,20 +1,10 @@ +using Plots plotly() -sys = feedback(1.0, ss(-1.0, 2, 1, 0) * (delay(2.0) + delay(3.0))) +sys = feedback(1.0, ss(-1.0, 2, 1, 0) * (delay(2.0) + delay(3.0) + delay(2.5))) -@time t, x, saved_y = ControlSystems.simulate5(sys, (0.0,8.0); u=t->[t>0 ? 1.0 : 0.0], saveat=0:0.02:8) - -y = hcat([saved_y.saveval[k][1] for k=1:length(t)]...) -d = hcat([saved_y.saveval[k][2] for k=1:length(t)]...) - -for k=1:length(sys.Tau) - N_del = Integer(50*sys.Tau[k]) - dk = [zeros(N_del); d[k, 1:end-N_del]] - - for j=1:length(t) - y[:, j] .+= sys.P.D12[:, k] * dk[j] - end -end +t = 0:0.02:8 +@time t, x, y = lsim(sys, t; u=t->[t>=0 ? 1.0 : 0.0]) plot(t, y') gui() diff --git a/src/delay_systems.jl b/src/delay_systems.jl index 8651ceeb8..6a34d3189 100644 --- a/src/delay_systems.jl +++ b/src/delay_systems.jl @@ -23,41 +23,51 @@ end -function simulate5(sys::DelayLtiSystem, tspan; u=[], x0=Float64[], alg=MethodOfSteps(Tsit5()), kwargs...) +function lsim(sys::DelayLtiSystem, t::AbstractArray{<:Real}; u=(t -> fill(0.0, ninputs(sys))), x0=fill(0.0, nstates(sys)), alg=MethodOfSteps(Tsit5()), kwargs...) P = sys.P if ~iszero(P.D22) error("non-zero D22-matrix block is not supported") # Due to limitations in differential equations end - nu = ControlSystems.ninputs(sys) - nx = ControlSystems.nstates(sys) - - u = (u == []) ? t -> fill(0.0, nu) : u - x0 = (x0 == []) ? fill(0.0, nx) : x0 + dt = t[2] - t[1] + if ~all(diff(t) .≈ dt) # QUESTION Does this work or are there precision problems? + error("The t-vector should be uniformly spaced, t[2] - t[1] = $dt.") # Perhaps dedicated function for checking this? + end + # Slightly more complicated definition since the d signal is not directly available dde = function (dx, x, h, p, t) dx .= P.A*x + P.B1*u(t) for k=1:length(sys.Tau) # Add each of the delayed signals - dk_delayed = dot(P.C2[k,:], h(p,t-sys.Tau[k])) + dot(P.D21[k,:], u(t-sys.Tau[k]))# + P.D22*(t-Tau[1] + dk_delayed = dot(P.C2[k,:], h(p,t-sys.Tau[k])) + dot(P.D21[k,:], u(t-sys.Tau[k])) dx .+= P.B2[:, k] * dk_delayed end - #d_delayed = zeros(size(P.C2,1)) end - h_initial = (p, t) -> zeros(nx) - + h_initial = (p, t) -> zeros(nstates(sys)) # Saves y (excluding the d(t-τ) contribution) and d - saved_values_y = SavedValues(Float64, Tuple{Vector{Float64}, Vector{Float64}}) - cb = SavingCallback((x,t,integrator) -> (P.C1*x + P.D11*u(t), P.C2*x + P.D21*u(t)), saved_values_y, saveat=0:0.02:tspan[2]) -#P.C1*x + P.D11*u(t) + P.D12*h(t) - prob = DDEProblem(dde, x0, h_initial, tspan, constant_lags=sys.Tau) + saved_values = SavedValues(Float64, Tuple{Vector{Float64}, Vector{Float64}}) + cb = SavingCallback((x,t,integrator) -> (P.C1*x + P.D11*u(t), P.C2*x + P.D21*u(t)), saved_values, saveat=t) + + prob = DDEProblem(dde, x0, h_initial, (0.0, 8.0), constant_lags=sys.Tau) - sol = solve(prob, alg, callback=cb; saveat=0:0.02:tspan[2], kwargs...) + sol = solve(prob, alg, callback=cb; saveat=t, kwargs...) - t, x = sol.t, sol.u + x = sol.u # the states are labeled u in DifferentialEquations + y = hcat([saved_values.saveval[k][1] for k=1:length(t)]...) + d = hcat([saved_values.saveval[k][2] for k=1:length(t)]...) + + # Account for the effect of the delayed d-signal on y + for k=1:length(sys.Tau) + N_del = Integer(sys.Tau[k] / dt) + dk = [zeros(N_del); d[k, 1:end-N_del]] + + for j=1:length(t) + y[:, j] .+= sys.P.D12[:, k] * dk[j] + end + end - t, x, saved_values_y + t, x, y end diff --git a/src/types/PartionedStateSpace.jl b/src/types/PartionedStateSpace.jl index cffe82df3..84586569c 100644 --- a/src/types/PartionedStateSpace.jl +++ b/src/types/PartionedStateSpace.jl @@ -119,12 +119,10 @@ function feedback(s1::PartionedStateSpace, s2::PartionedStateSpace) # For the case of two outputs # X_12 = [I -s2.D11 -s2.D11*s1.D12 -s2.D12] - # X_22 = [s1.D11 I -s1.D12 s1.D11*s2.D12] + # X_22 = [s1.D11 I s1.D12 -s1.D11*s2.D12] X_12 = (I + s2.D11*s1.D11)\[I -s2.D11*s1.D12 -s2.D12] X_22 = (I + s1.D11*s2.D11)\[s1.D11 s1.D12 -s1.D11*s2.D12] - - A = [s1.B1 * X_11 ; s2.B1 * X_21] + blkdiag(s1.A, s2.A) B = [s1.B1 * X_12 ; s2.B1 * X_22] diff --git a/test/runtests.jl b/test/runtests.jl index b415db570..fac0ea4c0 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -23,7 +23,9 @@ my_tests = ["test_statespace", "test_analysis", "test_matrix_comps", "test_lqg", - "test_synthesis"] + "test_synthesis", + "test_partitioned_statespace", + "test_delayed_systems"] # try diff --git a/test/test_delayed_systems.jl b/test/test_delayed_systems.jl index 710e6c589..54422e0d2 100644 --- a/test/test_delayed_systems.jl +++ b/test/test_delayed_systems.jl @@ -4,6 +4,7 @@ @test typeof(promote(delay(0.2), ss(1.0 + im))[1]) == DelayLtiSystem{Complex{Float64}} +# Extremely baseic tests @test freqresp(delay(1), ω)[:] ≈ exp.(-im*ω) rtol=1e-15 @test freqresp(delay(2.5), ω)[:] ≈ exp.(-2.5im*ω) rtol=1e-15 @test freqresp(3.5*delay(2.5), ω)[:] ≈ 3.5*exp.(-2.5im*ω) rtol=1e-15 @@ -19,14 +20,15 @@ P2 = DelayLtiSystem(ss(-2.0, -1, 1, 1)) # (s+1)/(s+2) P2_fr = (im*ω .+ 1) ./ (im*ω .+ 2) @test freqresp(P2, ω)[:] ≈ P2_fr rtol=1e-15 -# Addition + +## Addition @test_broken freqresp(1 + delay(1), ω)[:] ≈ 1 .+ exp.(-im*ω) # FIXME; Add suitable conversion from Int64 to DelayLtiSystem @test freqresp(P1 + delay(1), ω)[:] ≈ P1_fr .+ exp.(-im*ω) #FIXME: the following gives a crash.. freqresp(P1 - delay(1), ω)[:] ≈ P1_fr .+ exp.(-im*ω) -# Multiplication +## Multiplication @test freqresp(P1 * delay(1), ω)[:] ≈ P1_fr .* exp.(-im*ω) rtol=1e-15 @test freqresp(delay(1) * P1, ω)[:] ≈ P1_fr .* exp.(-im*ω) rtol=1e-15 @@ -34,7 +36,7 @@ P2_fr = (im*ω .+ 1) ./ (im*ω .+ 2) @test freqresp(delay(1) * P2, ω)[:] ≈ P2_fr .* exp.(-im*ω) rtol=1e-15 -# Feedback +## Feedback # The first tests don't include delays, but the linear system is of DelayLtiForm type # (very simple system so easy to troubleshoot) @test freqresp(feedback(1.0, P1), ω)[:] ≈ 1 ./ (1 .+ P1_fr) rtol=1e-15 @@ -44,11 +46,6 @@ P2_fr = (im*ω .+ 1) ./ (im*ω .+ 2) @test freqresp(feedback(1.0, DelayLtiSystem(ss(0.5))), [0])[:] == [2/3] @test freqresp(feedback(1.0, P2), ω)[:] ≈ 1 ./ (1 .+ P2_fr) - -freqresp(feedback(1.0, ss(0.5) + 0.5*delay(2) + 0.5*delay(3)), ω)[:] -freqresp(feedback(1.0, G), ω)[:] - - @test freqresp(feedback(0.5, delay(2.0)), ω) ≈ 0.5 ./ (1 .+ 0.5*exp.(-2im*ω)) @test freqresp(feedback(delay(2.0), 0.5), ω) ≈ exp.(-2im*ω) ./ (1 .+ 0.5*exp.(-2im*ω)) @@ -71,9 +68,6 @@ G_fr = 0.5 .+ 0.5*exp.(-2*im*ω)# + 0.5*exp.(-3*im*ω) @test freqresp(feedback(P2, G), ω)[:] ≈ P2_fr ./(1 .+ P2_fr .* G_fr) rtol=1e-15 -sys = feedback(1.0, P1 * (delay(2) + delay(3))) -expected_sys_fr = 1.0 ./ (P1_fr .* (exp.(-2*im*ω) + exp.(-3*im*ω)) .+ 1) -@test freqresp(sys, ω)[:] ≈ expected_sys_fr rtol=1e-14 ## Multiple Delays G = ss(1.0) + delay(2) + delay(3) @@ -81,5 +75,12 @@ G_fr = 1 .+ exp.(-2*im*ω) .+ exp.(-3*im*ω) @test freqresp(G, ω)[:] ≈ G_fr rtol=1e-15 @test freqresp(feedback(1.0, G), ω)[:] ≈ 1 ./(1 .+ G_fr) # Somewhat pathological system though +sys = feedback(1.0, P1 * (delay(2) + delay(3))) +expected_sys_fr = 1.0 ./ (P1_fr .* (exp.(-2*im*ω) + exp.(-3*im*ω)) .+ 1) + +@test freqresp(sys, ω)[:] ≈ expected_sys_fr rtol=1e-14 + +@test freqresp(feedback(1.0*P1, G*G*P2*P2), ω)[:] ≈ P1_fr ./(1 .+ (G_fr.^2).*P1_fr.*P2_fr.^2) rtol=1e-15# Somewhat pathological system though + #FIXME: A lot more tests, including MIMO systems in particular diff --git a/test/test_partitioned_statespace.jl b/test/test_partitioned_statespace.jl index cee4a5034..03dd68a1c 100644 --- a/test/test_partitioned_statespace.jl +++ b/test/test_partitioned_statespace.jl @@ -23,19 +23,18 @@ sys1.D22 == matrix(9.0) ## -sys1 = ControlSystems.PartionedStateSpace(ss(fill(1.0, 2, 2), fill(2.0, 2, 5), fill(3.0, 7, 2), fill(4.0, 7, 5)), 2, 3) - -sys1.A == fill(1.0, 2, 2) -sys1.B1 == fill(2.0, 2, 2) -sys1.B2 == fill(2.0, 2, 3) -sys1.C1 == fill(3.0, 3, 2) -sys1.C2 == fill(3.0, 4, 2) -sys1.D11 == fill(4.0, 3, 2) -sys1.D12 == fill(4.0, 3, 3) -sys1.D21 == fill(4.0, 4, 2) -sys1.D22 == fill(4.0, 4, 3) - - -# Simple test -(sys1 + sys1).P[1, 1] == (sys1.P[1,1] + sys1.P[1,1]) -(sys1 * sys1).P[1, 1] == (sys1.P[1,1] * sys1.P[1,1]) +sys2 = ControlSystems.PartionedStateSpace(ss(fill(1.0, 2, 2), fill(2.0, 2, 5), fill(3.0, 7, 2), fill(4.0, 7, 5)), 2, 3) + +@test sys2.A == fill(1.0, 2, 2) +@test sys2.B1 == fill(2.0, 2, 2) +@test sys2.B2 == fill(2.0, 2, 3) +@test sys2.C1 == fill(3.0, 3, 2) +@test sys2.C2 == fill(3.0, 4, 2) +@test sys2.D11 == fill(4.0, 3, 2) +@test sys2.D12 == fill(4.0, 3, 3) +@test sys2.D21 == fill(4.0, 4, 2) +@test sys2.D22 == fill(4.0, 4, 3) + + +# TODO: Add some tests for interconnections, implicitly tested through delay system implementations though +@test (sys1 + sys1).P[1, 1] == (sys1.P[1,1] + sys1.P[1,1]) From 2a574aa8c49f719e2aedd614c9b56f77c50a4cd2 Mon Sep 17 00:00:00 2001 From: olof3 Date: Thu, 24 Jan 2019 17:12:36 +0100 Subject: [PATCH 07/30] Minor typo. --- src/types/PartionedStateSpace.jl | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/types/PartionedStateSpace.jl b/src/types/PartionedStateSpace.jl index 84586569c..d3c954ae6 100644 --- a/src/types/PartionedStateSpace.jl +++ b/src/types/PartionedStateSpace.jl @@ -111,8 +111,7 @@ end -# QUESTION: Should perhaps algebraic loops?! Perhaps issue warning if P1(∞)*P2(∞) > 1 - +# QUESTION: What about algebraic loops and well-posedness?! Perhaps issue warning if P1(∞)*P2(∞) > 1 function feedback(s1::PartionedStateSpace, s2::PartionedStateSpace) X_11 = (I + s2.D11*s1.D11)\[-s2.D11*s1.C1 -s2.C1] X_21 = (I + s1.D11*s2.D11)\[s1.C1 -s1.D11*s2.C1] From 5da7529373fc23de16c20b3e838644108794e07e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mattias=20F=C3=A4lt?= Date: Fri, 1 Feb 2019 21:07:11 +0100 Subject: [PATCH 08/30] WIP delayed lsim --- REQUIRE | 2 +- example/delayed_lti_timeresp.jl | 18 ++++- src/ControlSystems.jl | 3 +- src/delay_systems.jl | 123 ++++++++++++++++++++++++------- src/types/PartionedStateSpace.jl | 2 +- 5 files changed, 119 insertions(+), 29 deletions(-) diff --git a/REQUIRE b/REQUIRE index 1deda6f8d..7c3ca060f 100644 --- a/REQUIRE +++ b/REQUIRE @@ -2,9 +2,9 @@ julia 0.7 Plots Polynomials LaTeXStrings -DifferentialEquations DelayDiffEq OrdinaryDiffEq IterTools Colors DSP +Interpolations diff --git a/example/delayed_lti_timeresp.jl b/example/delayed_lti_timeresp.jl index a7e44e591..4b4a46681 100644 --- a/example/delayed_lti_timeresp.jl +++ b/example/delayed_lti_timeresp.jl @@ -4,7 +4,23 @@ plotly() sys = feedback(1.0, ss(-1.0, 2, 1, 0) * (delay(2.0) + delay(3.0) + delay(2.5))) t = 0:0.02:8 -@time t, x, y = lsim(sys, t; u=t->[t>=0 ? 1.0 : 0.0]) +@time lsim(sys, t-> [t>=0 ? 1.0 : 0.0], t) +@time t, x, y = lsim(sys, [1.0], t) +@time t, x, y = lsim(sys, (out, t) -> (out .= (t>=0 ? 1.0 : 0.0)), t) +@time t, x, y = lsim(sys, (out, t) -> (out[1] = (t>=0 ? 1.0 : 0.0)), t) + +function u0(out,t) + if t > 0 + out[1] = 1 + else + out[1] = 0 + end + return +end + +@time t, x, y = lsim(sys, t; u=u0) plot(t, y') gui() + +@time ControlSystems._lsim(sys, t, (out,t)-> out[1] = (t>=0 ? 1.0 : 0.0)) diff --git a/src/ControlSystems.jl b/src/ControlSystems.jl index 94760ef4b..402a71130 100644 --- a/src/ControlSystems.jl +++ b/src/ControlSystems.jl @@ -79,11 +79,12 @@ export LTISystem, # QUESTION: are these used? LaTeXStrings, Requires, IterTools using Polynomials, Plots, LaTeXStrings, LinearAlgebra -using DifferentialEquations, OrdinaryDiffEq, DelayDiffEq +using OrdinaryDiffEq, DelayDiffEq export Plots import Base: +, -, *, /, (==), (!=), isapprox, convert, promote_op import Base: getproperty import LinearAlgebra: BlasFloat +import Interpolations export lyap # Make sure LinearAlgebra.lyap is available import Printf, Colors import DSP: conv diff --git a/src/delay_systems.jl b/src/delay_systems.jl index 6a34d3189..a530e8fde 100644 --- a/src/delay_systems.jl +++ b/src/delay_systems.jl @@ -21,9 +21,67 @@ function freqresp(sys::DelayLtiSystem, ω::AbstractVector{T}) where {T <: Real} return G_fr end +struct FunctionWrapper <: Function + f::Function +end +(fv::FunctionWrapper)(dx, x, h!, p, t) = fv.f(dx, x, h!, p, t) + +struct UWrapper <: Function + f::Function +end +(fv::UWrapper)(out, t) = fv.f(out, t) + + +""" + `t, x, y = lsim(sys::DelayLtiSystem, t::AbstractArray{<:Real}; u=(out, t) -> (out .= 0), x0=fill(0.0, nstates(sys)), alg=MethodOfSteps(Tsit5()), kwargs...)` + + Simulate system `sys`, over time `t`, using input signal `u`, with initial state `x0`, using method `alg` . + + Arguments: + + `t`: Has to be an `AbstractVector` with equidistant time samples (`t[i] - t[i-1]` constant) + `u`: Function to determine control signal `ut` at a time `t`, on any of the following forms: + Can be a constant `Number` or `Vector`, interpreted as `ut .= u` , or + Function `ut .= u(t)`, or + In-place function `u(ut, t)`. (Slightly more effienct) + + Returns: `x` and `y` at times `t`. +""" +function lsim(sys::DelayLtiSystem{T}, u, t::AbstractArray{<:T}; x0=fill(zero(T), nstates(sys)), alg=MethodOfSteps(Tsit5())) where T + # Make u! in-place function of u + u! = if isa(u, Number) || isa(u,AbstractVector) # Allow for u to be a constant number or vector + println("Number vector") + (uout, t) -> uout .= u + elseif DiffEqBase.isinplace(u, 2) # If u is an inplace (more than 1 argument function) + println("Inplace") + u + else # If u is a regular u(t) function + println("Outplace") + (out, t) -> (out .= u(t)) + end + _lsim(sys, UWrapper(u!), t, x0, alg) +end +function dde_param(dx, x, h!, p, t) + A, B1, B2, C2, D21, Tau, u!, uout, hout, tmp = p[1],p[2],p[3],p[4],p[5],p[6],p[7],p[8],p[9],p[10] + + u!(uout, t) # uout = u(t) + + #dx .= A*x + B1*ut + mul!(dx, A, x) + mul!(tmp, B1, uout) + dx .+= tmp + + for k=1:length(Tau) # Add each of the delayed signals + u!(uout, t-Tau[k]) # uout = u(t-tau[k]) + h!(hout, p, t-Tau[k]) + dk_delayed = dot(view(C2,k,:), hout) + dot(view(D21,k,:), uout) + dx .+= view(B2,:, k) .* dk_delayed + end + return +end -function lsim(sys::DelayLtiSystem, t::AbstractArray{<:Real}; u=(t -> fill(0.0, ninputs(sys))), x0=fill(0.0, nstates(sys)), alg=MethodOfSteps(Tsit5()), kwargs...) +function _lsim(sys::DelayLtiSystem{T}, u!, t::AbstractArray{<:T}, x0::Vector{T}, alg) where T P = sys.P if ~iszero(P.D22) @@ -35,39 +93,54 @@ function lsim(sys::DelayLtiSystem, t::AbstractArray{<:Real}; u=(t -> fill(0.0, n error("The t-vector should be uniformly spaced, t[2] - t[1] = $dt.") # Perhaps dedicated function for checking this? end - # Slightly more complicated definition since the d signal is not directly available - dde = function (dx, x, h, p, t) - dx .= P.A*x + P.B1*u(t) - for k=1:length(sys.Tau) # Add each of the delayed signals - dk_delayed = dot(P.C2[k,:], h(p,t-sys.Tau[k])) + dot(P.D21[k,:], u(t-sys.Tau[k])) - dx .+= P.B2[:, k] * dk_delayed - end - end + # Get all matrices to save on allocations + A, B1, B2, C1, C2, D11, D12, D21, D22 = P.A, P.B1, P.B2, P.C1, P.C2, P.D11, P.D12, P.D21, P.D22 + Tau = sys.Tau - h_initial = (p, t) -> zeros(nstates(sys)) + hout = fill(zero(T), nstates(sys)) # in place storage for h + uout = fill(zero(T), ninputs(sys)) # in place storage for u + tmp = similar(x0) - # Saves y (excluding the d(t-τ) contribution) and d - saved_values = SavedValues(Float64, Tuple{Vector{Float64}, Vector{Float64}}) - cb = SavingCallback((x,t,integrator) -> (P.C1*x + P.D11*u(t), P.C2*x + P.D21*u(t)), saved_values, saveat=t) + h!(out, p, t) = (out .= 0) # History function - prob = DDEProblem(dde, x0, h_initial, (0.0, 8.0), constant_lags=sys.Tau) + p = (A, B1, B2, C2, D21, Tau, u!, uout, hout, tmp) + prob = DDEProblem{true}(dde_param, x0, h!, (t[1], t[end]), p, constant_lags=sys.Tau) - sol = solve(prob, alg, callback=cb; saveat=t, kwargs...) + sol = DelayDiffEq.solve(prob, alg, saveat=t) - x = sol.u # the states are labeled u in DifferentialEquations - y = hcat([saved_values.saveval[k][1] for k=1:length(t)]...) - d = hcat([saved_values.saveval[k][2] for k=1:length(t)]...) + x = sol.u::Array{Array{T,1},1} # the states are labeled u in DelayDiffEq + println(size(x)) - # Account for the effect of the delayed d-signal on y - for k=1:length(sys.Tau) - N_del = Integer(sys.Tau[k] / dt) - dk = [zeros(N_del); d[k, 1:end-N_del]] + y = Array{T,2}(undef, noutputs(sys), length(t)) + d = Array{T,2}(undef, size(C2,1), length(t)) + # Build y signal (without d term) + for k = 1:length(t) + u!(uout, t[k]) + y[:,k] = C1*x[k] + D11*uout + #d[:,k] = C2*x[k] + D21*uout + end + xitp = Interpolations.interpolate((t,), x, Interpolations.Gridded(Interpolations.Linear())) + + dtmp = Array{T,1}(undef, size(C2,1)) + # Function to evaluate d(t)_i at an arbitrary time + # X is constinuous, so interpoate, u is not + function dfunc!(tmp::Array{T1}, t, i) where T1 + tmp .= if t < 0 + T1(0) + else + xitp(t) + end + u!(uout, t) + return dot(view(C2,i,:),tmp) + dot(view(D21,i,:),uout) + end + # Account for the effect of the delayed d-signal on y + for k=1:length(Tau) for j=1:length(t) - y[:, j] .+= sys.P.D12[:, k] * dk[j] + di = dfunc!(tmp, t[j] - Tau[k], k) + y[:, j] .+= view(D12,:, k) .* di end end - - t, x, y + return t, x, y end diff --git a/src/types/PartionedStateSpace.jl b/src/types/PartionedStateSpace.jl index d3c954ae6..b42294c66 100644 --- a/src/types/PartionedStateSpace.jl +++ b/src/types/PartionedStateSpace.jl @@ -60,7 +60,7 @@ function blkdiag(A1::Matrix{T1}, A2::Matrix{T2}) where {T1<:Number, T2<:Number} dims1 = size(A1) dims2 = size(A2) - A_new = zeros(Float64, dims1 .+ dims2) + A_new = zeros(T, dims1 .+ dims2) A_new[1:dims1[1], 1:dims1[2]] = A1 A_new[dims1[1]+1:end, dims1[2]+1:end] = A2 From cc0deecfa28d55e6185104201a790fec2c6e4bfd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mattias=20F=C3=A4lt?= Date: Sat, 2 Feb 2019 16:00:18 +0100 Subject: [PATCH 09/30] Working version of delay systems --- example/delayed_lti_timeresp.jl | 72 ++++++++++++++++--- src/connections.jl | 24 +++++++ src/delay_systems.jl | 58 +++++++++++++-- src/plotting.jl | 14 ++++ src/types/DelayLtiSystem.jl | 80 +++++++++++++++++++-- src/types/Lti.jl | 27 +++++++ src/types/PartionedStateSpace.jl | 117 ++++++++++++++++++++++++------- src/types/StateSpace.jl | 1 - src/types/TransferFunction.jl | 1 - src/types/conversion.jl | 22 ++++-- src/types/promotion.jl | 15 ++++ test/test_delayed_systems.jl | 56 +++++++++++++-- 12 files changed, 428 insertions(+), 59 deletions(-) diff --git a/example/delayed_lti_timeresp.jl b/example/delayed_lti_timeresp.jl index 4b4a46681..0753c3480 100644 --- a/example/delayed_lti_timeresp.jl +++ b/example/delayed_lti_timeresp.jl @@ -1,13 +1,14 @@ -using Plots -plotly() +using ControlSystems, Plots +gr() sys = feedback(1.0, ss(-1.0, 2, 1, 0) * (delay(2.0) + delay(3.0) + delay(2.5))) +sys = feedback(ss(-1.0, 1, 1, 0), delay(1.0)) t = 0:0.02:8 -@time lsim(sys, t-> [t>=0 ? 1.0 : 0.0], t) -@time t, x, y = lsim(sys, [1.0], t) -@time t, x, y = lsim(sys, (out, t) -> (out .= (t>=0 ? 1.0 : 0.0)), t) -@time t, x, y = lsim(sys, (out, t) -> (out[1] = (t>=0 ? 1.0 : 0.0)), t) +@time y, t, x = lsim(sys, t-> [t>=0 ? 1.0 : 0.0], t) +@time y, t, x = lsim(sys, [1.0], t) +@time y, t, x = lsim(sys, (out, t) -> (out .= (t>=0 ? 1.0 : 0.0)), t) +@time y, t, x = lsim(sys, (out, t) -> (out[1] = (t>=0 ? 1.0 : 0.0)), t) function u0(out,t) if t > 0 @@ -18,9 +19,62 @@ function u0(out,t) return end -@time t, x, y = lsim(sys, t; u=u0) +@time y, t, x = lsim(sys, u0, t) plot(t, y') -gui() -@time ControlSystems._lsim(sys, t, (out,t)-> out[1] = (t>=0 ? 1.0 : 0.0)) +s = tf("s") +P = delay(2.6)*ss((s+3.0)/(s^2+0.3*s+1)) +C = 0.06 * ss(1.0 + 1/s); +P*C +T = feedback(P*C,1.0) + +t = 0:0.1:70 +y, t, x = lsim(T, t -> (t<0 ? 0 : 1 ), t) +plot(t, y, c = :blue) + +w = 10 .^ (-2:0.01:2) +marginplot(P*C, w) +marginplot(P*C) + +notch = ss(tf([1, 0.2, 1],[1, .8, 1])); +C = ss(0.05 * (1 + 1/s)); +Tnotch = feedback(P*C*notch, 1.0) + +stepplot(Tnotch) + +y, t, x = step(C, method=:zoh) + +y2, t2, x2 = step(Tnotch) +stepplot(Tnotch) + +stepplot(Tnotch, 40, 0.1) + +stepplot(T, 100) + +G = delay(5)/(s+1) +T = feedback(G, 0.5) +w = 10 .^ (-2:0.01:3) +bodeplot(T, w, plotphase=false) + +# Test conversion, promotion +delay(1,Int64) + 3.5 + +G = 1 + 0.5 * delay(3) +w = 10 .^(-2:0.001:2) +bodeplot(G, w, plotphase=false) + +G = delay(1) * ((0.8*s^2+s+2)/(s^2+s)) +T = feedback(G,1) +# Not possible with direct term +stepplot(T) + +bodeplot(T) + +G = 1/(s+1) + delay(4) +T = feedback(1,G) +# Not possible to lsim with direct term +stepplot(T) +bodeplot(T) + +s = tf("s") diff --git a/src/connections.jl b/src/connections.jl index 01f2fcf40..e04515747 100644 --- a/src/connections.jl +++ b/src/connections.jl @@ -43,6 +43,24 @@ end append(systems::LTISystem...) = append(promote(systems...)...) +# TODO Move size check to wrappers + +function Base.vcat(systems::DelayLtiSystem...) + for sys in systems + println(sys.P.ny1) + end + P = vcat_1([sys.P for sys in systems]...) # See PartitionedStateSpace + Tau = vcat([sys.Tau for sys in systems]...) + return DelayLtiSystem(P, Tau) +end + +function Base.hcat(systems::DelayLtiSystem...) + P = hcat_1([sys.P for sys in systems]...) # See PartitionedStateSpace + Tau = vcat([sys.Tau for sys in systems]...) + return DelayLtiSystem(P, Tau) +end + + function Base.vcat(systems::StateSpace...) # Perform checks nu = systems[1].nu @@ -124,6 +142,12 @@ Base.typed_hcat(::Type{T}, X...) where {T<:LTISystem} = hcat(convert.(T, X)...) # Ambiguity Base.typed_hcat(::Type{T}, X::Number...) where {T<:LTISystem, N} = hcat(convert.(T, X)...) +# Catch special cases where inv(sys) might not be possible after promotion, like improper tf +function /(sys1::Union{StateSpace,DelayLtiSystem}, sys2::LTISystem) + sys1new, sys2new = promote(sys1, 1/sys2) + return sys1new*sys2new +end + # function hvcat(rows::Tuple{Vararg{Int}}, systems::Union{Number,AbstractVecOrMat{<:Number},LTISystem}...) # T = Base.promote_typeof(systems...) # nbr = length(rows) # number of block rows diff --git a/src/delay_systems.jl b/src/delay_systems.jl index a530e8fde..5a4a5d609 100644 --- a/src/delay_systems.jl +++ b/src/delay_systems.jl @@ -33,7 +33,7 @@ end """ - `t, x, y = lsim(sys::DelayLtiSystem, t::AbstractArray{<:Real}; u=(out, t) -> (out .= 0), x0=fill(0.0, nstates(sys)), alg=MethodOfSteps(Tsit5()), kwargs...)` + `y, t, x = lsim(sys::DelayLtiSystem, t::AbstractArray{<:Real}; u=(out, t) -> (out .= 0), x0=fill(0.0, nstates(sys)), alg=MethodOfSteps(Tsit5()), kwargs...)` Simulate system `sys`, over time `t`, using input signal `u`, with initial state `x0`, using method `alg` . @@ -45,7 +45,7 @@ end Function `ut .= u(t)`, or In-place function `u(ut, t)`. (Slightly more effienct) - Returns: `x` and `y` at times `t`. + Returns: times `t`, and `y` and `x` at those times. """ function lsim(sys::DelayLtiSystem{T}, u, t::AbstractArray{<:T}; x0=fill(zero(T), nstates(sys)), alg=MethodOfSteps(Tsit5())) where T # Make u! in-place function of u @@ -88,6 +88,7 @@ function _lsim(sys::DelayLtiSystem{T}, u!, t::AbstractArray{<:T}, x0::Vector{T}, error("non-zero D22-matrix block is not supported") # Due to limitations in differential equations end + t0 = first(t) dt = t[2] - t[1] if ~all(diff(t) .≈ dt) # QUESTION Does this work or are there precision problems? error("The t-vector should be uniformly spaced, t[2] - t[1] = $dt.") # Perhaps dedicated function for checking this? @@ -125,7 +126,7 @@ function _lsim(sys::DelayLtiSystem{T}, u!, t::AbstractArray{<:T}, x0::Vector{T}, # Function to evaluate d(t)_i at an arbitrary time # X is constinuous, so interpoate, u is not function dfunc!(tmp::Array{T1}, t, i) where T1 - tmp .= if t < 0 + tmp .= if t < t0 T1(0) else xitp(t) @@ -142,5 +143,54 @@ function _lsim(sys::DelayLtiSystem{T}, u!, t::AbstractArray{<:T}, x0::Vector{T}, end end - return t, x, y + return y', t, hcat(x...)' +end + + +# We have to default to something, look at the sys.P.P and delays +function _bounds_and_features(sys::DelayLtiSystem, plot::Symbol) + ws, pz = _bounds_and_features(sys.P.P, plot) + logtau = log10.(abs.(sys.Tau)) + logtau = logtau[logtau .> -4] # Ignore low frequency + if isempty(logtau) + return ws, pz + end + extreme = extrema(logtau) + return [min(ws[1], floor(extreme[1]-0.2)), max(ws[2], ceil(extreme[2]+0.2))], pz +end + +# Againm we have to do something for default vectors, more or less a copy from timeresp.jl +function _default_Ts(sys::DelayLtiSystem) + if !isstable(sys.P.P) + return 0.05 # Something small + else + ps = pole(sys.P.P) + r = minimum([abs.(real.(ps));0]) # Find the fastest pole of sys.P.P + r = min(r, minimum([sys.Tau;0])) # Find the fastest delay + if r == 0.0 + r = 1.0 + end + return 0.07/r + end +end + +iscontinuous(sys::DelayLtiSystem) = true + +function Base.step(sys::DelayLtiSystem{T}, t::AbstractVector, kwargs...) where T + nu = ninputs(sys) + if t[1] != 0 + throw(ArgumentError("First time point must be 0 in step")) + end + u = (out, t) -> (t < 0 ? out .= 0 : out .= 1) + x0=fill(zero(T), nstates(sys)) + if nu == 1 + y, tout, x = lsim(sys, u, t, x0=x0, kwargs...) + else + x = Array{T}(undef, length(t), nstates(sys), nu) + y = Array{T}(undef, length(t), noutputs(sys), nu) + for i=1:nu + y[:,:,i], tout, x[:,:,i] = lsim(sys[:,i], u, t, x0=x0, kwargs...) + end + end + return y, tout, x end diff --git a/src/plotting.jl b/src/plotting.jl index 62beec140..8896de9c0 100644 --- a/src/plotting.jl +++ b/src/plotting.jl @@ -625,6 +625,20 @@ function marginplot(systems::Union{AbstractVector{T},T}, args...; kwargs...) whe for j=1:nu for i=1:ny wgm, gm, wpm, pm, fullPhase = sisomargin(s[i,j],w, full=true, allMargins=true) + # Let's be reasonable, only plot 5 smallest gain margins + if length(gm) > 5 + @warn "Only showing smallest 5 out of $(length(gm)) gain margins" + idx = sortperm(gm) + wgm = wgm[idx[1:5]] + gm = gm[idx[1:5]] + end + # Let's be reasonable, only plot 5 smallest phase margins + if length(pm) > 5 + @warn "Only showing \"smallest\" 5 out of $(length(pm)) phase margins" + idx = sortperm(pm) + wgm = wpm[idx[1:5]] + gm = pm[idx[1:5]] + end if _PlotScale == "dB" mag = 20 .* log10.(1 ./ gm) oneLine = 0 diff --git a/src/types/DelayLtiSystem.jl b/src/types/DelayLtiSystem.jl index 24e7c9713..aba3880f6 100644 --- a/src/types/DelayLtiSystem.jl +++ b/src/types/DelayLtiSystem.jl @@ -13,7 +13,7 @@ end # QUESTION: would psys be a good standard variable name for a PartionedStateSpace # and perhaps dsys for a delayed system, (ambigous with discrete system though) -function DelayLtiSystem{T}(sys::StateSpace, Tau::Vector{Float64}) where T +function DelayLtiSystem{T}(sys::StateSpace, Tau::Vector{Float64}) where T<:Number nu = ninputs(sys) - length(Tau) ny = noutputs(sys) - length(Tau) @@ -21,24 +21,32 @@ function DelayLtiSystem{T}(sys::StateSpace, Tau::Vector{Float64}) where T error("Time vector is too long.") end - psys = PartionedStateSpace(sys, ny, nu) + psys = PartionedStateSpace{StateSpace{T,Matrix{T}}}(sys, nu, ny) DelayLtiSystem{T}(psys, Tau) end +# For converting DelayLtiSystem{T} to different T +DelayLtiSystem{T}(sys::DelayLtiSystem) where T = DelayLtiSystem{T}(PartionedStateSpace{StateSpace{T,Matrix{T}}}(sys.P), Float64[]) +DelayLtiSystem{T}(sys::StateSpace) where T = DelayLtiSystem{T}(sys, Float64[]) +# From StateSpace, infer type DelayLtiSystem(sys::StateSpace{T,MT}, Tau::Vector{Float64}) where {T, MT} = DelayLtiSystem{T}(sys, Tau) DelayLtiSystem(sys::StateSpace{T,MT}) where {T, MT} = DelayLtiSystem{T}(sys, Float64[]) -DelayLtiSystem{T}(sys::StateSpace) where T = DelayLtiSystem{T}(sys, Float64[]) +# From TransferFunction, infer type TODO Use proper constructor instead of convert here when defined +DelayLtiSystem(sys::TransferFunction{S}) where {T,S<:SisoTf{T}} = DelayLtiSystem(convert(StateSpace{T, Matrix{T}}, sys)) # TODO: Think through these promotions and conversions -Base.promote_rule(::Type{<:StateSpace{T1}}, ::Type{DelayLtiSystem{T2}}) where {T1<:Number,T2<:Number} = DelayLtiSystem{promote_type(T1,T2)} Base.promote_rule(::Type{AbstractMatrix{T1}}, ::Type{DelayLtiSystem{T2}}) where {T1<:Number,T2<:Number} = DelayLtiSystem{promote_type(T1,T2)} Base.promote_rule(::Type{T1}, ::Type{DelayLtiSystem{T2}}) where {T1<:Number,T2<:Number} = DelayLtiSystem{promote_type(T1,T2)} #Base.promote_rule(::Type{<:UniformScaling}, ::Type{S}) where {S<:DelayLtiSystem} = DelayLtiSystem{T} Base.convert(::Type{DelayLtiSystem{T}}, sys::StateSpace) where T = DelayLtiSystem{T}(sys) Base.convert(::Type{DelayLtiSystem{T1}}, d::T2) where {T1,T2 <: Number} = DelayLtiSystem{T1}(ss(d)) -Base.convert(::Type{DelayLtiSystem{T}}, sys::DelayLtiSystem) where T = DelayLtiSystem{T}(StateSpace{T,Matrix{T}}(sys.P.P), sys.Tau) +# Catch convertsion between T +Base.convert(::Type{S}, sys::DelayLtiSystem) where {T, S<:DelayLtiSystem{T}} = + sys isa S ? sys : S(StateSpace{T,Matrix{T}}(sys.P.P), sys.Tau) + +Base.convert(::Type{DelayLtiSystem{T}}, sys::TransferFunction) where T = DelayLtiSystem{T}(convert(StateSpace{T, Matrix{T}}, sys)) function *(sys::DelayLtiSystem, n::Number) @@ -48,10 +56,69 @@ function *(sys::DelayLtiSystem, n::Number) end *(n::Number, sys::DelayLtiSystem) = *(sys, n) +function +(sys::DelayLtiSystem{T1}, n::T2) where {T1,T2<:Number} + T = promote_type(T1,T2) + if T == T1 # T2 can be stored in sys + +(sys, T1(n)) + else # We need to upgrade sys + +(DelayLtiSystem{T}(sys), T(n)) + end +end + +# Efficient addition +function +(sys::DelayLtiSystem{T}, n::T) where {T<:Number} + ny, nu = size(sys) + ssold = sys.P.P + # Add to direct term from input to output + new_D = ssold.D + new_D[1:ny, 1:nu] .+= n + + pnew = PartionedStateSpace(StateSpace(ssold.A, ssold.B, ssold.C, new_D, 0.0), ny, nu) + DelayLtiSystem(pnew, sys.Tau) +end + +# Efficient subtraction with number +-(sys::DelayLtiSystem, n::T) where {T <:Number} = +(sys, -n) +-(n::T, sys::DelayLtiSystem) where {T <:Number} = +(-sys, n) + ninputs(sys::DelayLtiSystem) = size(sys.P.P, 2) - length(sys.Tau) noutputs(sys::DelayLtiSystem) = size(sys.P.P, 1) - length(sys.Tau) nstates(sys::DelayLtiSystem) = nstates(sys.P.P) +Base.size(sys::DelayLtiSystem) = (noutputs(sys), ninputs(sys)) + +# Fallbacks, TODO We should sort this out for all types, maybe after SISO/MIMO +# {Array, Number}, Colon +Base.getindex(sys::DelayLtiSystem, i, ::Colon) = + getindex(sys, index2range(i), 1:size(sys,2)) +# Colon, {Array, Number} +Base.getindex(sys::DelayLtiSystem, ::Colon, j) = + getindex(sys, 1:size(sys,1), index2range(j)) +Base.getindex(sys::DelayLtiSystem, ::Colon, ::Colon) = + getindex(sys, 1:size(sys,1), 1:size(sys,2)) +# Should just catch Number, Number, or Colon, Colon +Base.getindex(sys::DelayLtiSystem, i, j) = + getindex(sys, index2range(i), index2range(j)) + +function Base.getindex(sys::DelayLtiSystem, i::AbstractArray, j::AbstractArray) + ny, nu = size(sys) + # Cant use "boundscheck" since not AbstractArray + imin, imax = extrema(i) + jmin, jmax = extrema(j) + if imax > ny || imin < 1 || jmax > nu || jmin < 1 + throw(BoundsError(sys, (i,j))) + end + nrow, ncol = size(sys.P.P) + rowidx = [collect(i); collect((ny+1):nrow)] # Output plus delay terms + colidx = [collect(j); collect((nu+1):ncol)] # Input plus delay terms + DelayLtiSystem(StateSpace( + sys.P.A[:, :], + sys.P.B[:, colidx], + sys.P.C[rowidx, :], + sys.P.D[rowidx, colidx], + sys.P.Ts), sys.Tau) +end + function +(sys1::DelayLtiSystem, sys2::DelayLtiSystem) psys_new = sys1.P + sys2.P Tau_new = [sys1.Tau; sys2.Tau] @@ -59,6 +126,9 @@ function +(sys1::DelayLtiSystem, sys2::DelayLtiSystem) DelayLtiSystem(psys_new.P, Tau_new) end +-(sys1::DelayLtiSystem, sys2::DelayLtiSystem) = +(sys1, -sys2) +-(sys::DelayLtiSystem{T}) where {T} = *(sys, T(-1)) + function *(sys1::DelayLtiSystem, sys2::DelayLtiSystem) psys_new = sys1.P * sys2.P diff --git a/src/types/Lti.jl b/src/types/Lti.jl index 5fa9d197c..7bd8f2da0 100644 --- a/src/types/Lti.jl +++ b/src/types/Lti.jl @@ -4,6 +4,30 @@ abstract type LTISystem <: AbstractSystem end *(sys1::LTISystem, sys2::LTISystem) = *(promote(sys1, sys2)...) /(sys1::LTISystem, sys2::LTISystem) = /(promote(sys1, sys2)...) +# Fallback number ++(sys1::LTISystem, sys2::Number) = +(promote(sys1, sys2)...) +-(sys1::LTISystem, sys2::Number) = -(promote(sys1, sys2)...) +*(sys1::LTISystem, sys2::Number) = *(promote(sys1, sys2)...) +/(sys1::LTISystem, sys2::Number) = /(promote(sys1, sys2)...) + ++(sys1::Number, sys2::LTISystem) = +(promote(sys1, sys2)...) +-(sys1::Number, sys2::LTISystem) = -(promote(sys1, sys2)...) +*(sys1::Number, sys2::LTISystem) = *(promote(sys1, sys2)...) +/(sys1::Number, sys2::LTISystem) = /(promote(sys1, sys2)...) + +# Fallback Matrix ++(sys1::LTISystem, sys2::AbstractMatrix) = +(promote(sys1, sys2)...) +-(sys1::LTISystem, sys2::AbstractMatrix) = -(promote(sys1, sys2)...) +*(sys1::LTISystem, sys2::AbstractMatrix) = *(promote(sys1, sys2)...) +/(sys1::LTISystem, sys2::AbstractMatrix) = /(promote(sys1, sys2)...) + ++(sys1::AbstractMatrix, sys2::LTISystem) = +(promote(sys1, sys2)...) +-(sys1::AbstractMatrix, sys2::LTISystem) = -(promote(sys1, sys2)...) +*(sys1::AbstractMatrix, sys2::LTISystem) = *(promote(sys1, sys2)...) +/(sys1::AbstractMatrix, sys2::LTISystem) = /(promote(sys1, sys2)...) + +# TODO We should have proper fallbacks for matrices too + feedback(sys1::Union{LTISystem,Number,AbstractMatrix{<:Number}}, sys2::Union{LTISystem,Number,AbstractMatrix{<:Number}}) = feedback(promote(sys1, sys2)...) @@ -54,3 +78,6 @@ function _check_consistent_sampling_time(sys1::LTISystem, sys2::LTISystem) error("Sampling time mismatch") end end + +# Fallback since LTISystem not AbstractArray +Base.size(sys::LTISystem, i::Integer) = size(sys)[i] diff --git a/src/types/PartionedStateSpace.jl b/src/types/PartionedStateSpace.jl index b42294c66..5a3c63589 100644 --- a/src/types/PartionedStateSpace.jl +++ b/src/types/PartionedStateSpace.jl @@ -16,7 +16,9 @@ struct PartionedStateSpace{S} nu1::Int ny1::Int end - +# For converting between different S +PartionedStateSpace{S}(partsys::PartionedStateSpace) where {S<:StateSpace} = + PartionedStateSpace{S}(S(partsys.P), partsys.nu1, partsys.ny1) function getproperty(sys::PartionedStateSpace, d::Symbol) P = getfield(sys, :P) @@ -52,32 +54,16 @@ function getproperty(sys::PartionedStateSpace, d::Symbol) end end - -# There should already exist a function like this somewhare? -function blkdiag(A1::Matrix{T1}, A2::Matrix{T2}) where {T1<:Number, T2<:Number} - T = promote_type(T1, T2) - - dims1 = size(A1) - dims2 = size(A2) - - A_new = zeros(T, dims1 .+ dims2) - A_new[1:dims1[1], 1:dims1[2]] = A1 - A_new[dims1[1]+1:end, dims1[2]+1:end] = A2 - - return A_new -end - - function +(s1::PartionedStateSpace, s2::PartionedStateSpace) - A = blkdiag(s1.A, s2.A) + A = blockdiag(s1.A, s2.A) - B = [[s1.B1; s2.B1] blkdiag(s1.B2, s2.B2)] + B = [[s1.B1; s2.B1] blockdiag(s1.B2, s2.B2)] C = [[s1.C1 s2.C1]; - blkdiag(s1.C2, s2.C2)] + blockdiag(s1.C2, s2.C2)] D = [(s1.D11 + s2.D11) s1.D12 s2.D12; - [s1.D21; s2.D21] blkdiag(s1.D22, s2.D22)] + [s1.D21; s2.D21] blockdiag(s1.D22, s2.D22)] P = StateSpace(A, B, C, D, 0) # How to handle discrete? PartionedStateSpace(P, s1.nu1 + s2.nu1, s1.ny1 + s2.ny1) @@ -122,35 +108,112 @@ function feedback(s1::PartionedStateSpace, s2::PartionedStateSpace) X_12 = (I + s2.D11*s1.D11)\[I -s2.D11*s1.D12 -s2.D12] X_22 = (I + s1.D11*s2.D11)\[s1.D11 s1.D12 -s1.D11*s2.D12] - A = [s1.B1 * X_11 ; s2.B1 * X_21] + blkdiag(s1.A, s2.A) + A = [s1.B1 * X_11 ; s2.B1 * X_21] + blockdiag(s1.A, s2.A) B = [s1.B1 * X_12 ; s2.B1 * X_22] - tmp = blkdiag(s1.B2, s2.B2) + tmp = blockdiag(s1.B2, s2.B2) B[:, end-size(tmp,2)+1:end] .+= tmp C = [s1.D11 * X_11 ; s1.D21 * X_11 ; - s2.D21 * X_21 ] + [s1.C1 zeros(size(s1.C1,1),size(s2.C1,2)); blkdiag(s1.C2, s2.C2)] + s2.D21 * X_21 ] + [s1.C1 zeros(size(s1.C1,1),size(s2.C1,2)); blockdiag(s1.C2, s2.C2)] D = [s1.D11 * X_12 ; s1.D21 * X_12 ; s2.D21 * X_22 ] - tmp = [s1.D12 zeros(size(s1.D12,1),size(s2.D12,2)); blkdiag(s1.D22, s2.D22)] + tmp = [s1.D12 zeros(size(s1.D12,1),size(s2.D12,2)); blockdiag(s1.D22, s2.D22)] D[:, end-size(tmp,2)+1:end] .+= tmp # in case it is desired to consider both outputs # C = [s1.D11 * X_11 ; # s2.D11 * X_21 ; # s1.D21 * X_11 ; - # s2.D21 * X_21 ] + [blkdiag(s1.C1, s2.C1); blkdiag(s1.C2, s2.C2)] + # s2.D21 * X_21 ] + [blockdiag(s1.C1, s2.C1); blockdiag(s1.C2, s2.C2)] # # D = [s1.D11 * X_12 ; # s2.D11 * X_22 ; # s1.D21 * X_12 ; # s2.D21 * X_22 ] - #tmp = [blkdiag(s1.D12, s2.D12); blkdiag(s1.D22, s2.D22)] + #tmp = [blockdiag(s1.D12, s2.D12); blockdiag(s1.D22, s2.D22)] #D[:, end-size(tmp,2)+1:end] .+= tmp P = StateSpace(A, B, C, D, 0) PartionedStateSpace(P, s2.nu1, s1.ny1) end + +""" Concatenate systems vertically with + same first input u1 + second input [u2_1; u2_2 ...] + and output y1 = [y1_1; y1_2, ...] + y2 = [y2_1; y2_2, ...] + for u1_i, u2_i, y1_i, y2_i, where i denotes system i +""" +function vcat_1(systems::PartionedStateSpace...) + # Perform checks + println("vcal $(length(systems))") + println(systems) + Ts = systems[1].P.Ts + if !all(s.P.Ts == Ts for s in systems) + error("All systems have same sample time") + end + nu1 = systems[1].nu1 + if !all(s.nu1 == nu1 for s in systems) + error("All systems must have same first input dimension") + end + + A = blockdiag([s.A for s in systems]...) + + B1 = vcat([s.B1 for s in systems]...) + B2 = blockdiag([s.B2 for s in systems]...) + for i = 1:length(systems) + println(systems[i].ny1) + end + println("after") + C1 = blockdiag([s.C1 for s in systems]...) + C2 = blockdiag([s.C2 for s in systems]...) + + D11 = vcat([s.D11 for s in systems]...) + D12 = blockdiag([s.D12 for s in systems]...) + D21 = vcat([s.D21 for s in systems]...) + D22 = blockdiag([s.D22 for s in systems]...) + + sysnew = StateSpace(A, [B1 B2], [C1; C2], [D11 D12; D21 D22], Ts) + return PartionedStateSpace(sysnew, nu1, sum(s -> s.ny1, systems)) +end + + +""" Concatenate systems horizontally with + same first ouput y1 being the sum + second output y2 = [y2_1; y2_2, ...] + and inputs u1 = [u1_1; u1_2, ...] + u2 = [u2_1; u2_2, ...] + for u1_i, u2_i, y1_i, y2_i, where i denotes system i +""" +function hcat_1(systems::PartionedStateSpace...) + println("hcat $(length(systems))") + # Perform checks + Ts = systems[1].P.Ts + if !all(s.P.Ts == Ts for s in systems) + error("All systems have same sample time") + end + ny1 = systems[1].ny1 + if !all(s.ny1 == ny1 for s in systems) + error("All systems must have same first ouput dimension") + end + + A = blockdiag([s.A for s in systems]...) + + B1 = blockdiag([s.B1 for s in systems]...) + B2 = blockdiag([s.B2 for s in systems]...) + + C1 = hcat([s.C1 for s in systems]...) + C2 = blockdiag([s.C2 for s in systems]...) + + D11 = hcat([s.D11 for s in systems]...) + D12 = hcat([s.D12 for s in systems]...) + D21 = blockdiag([s.D21 for s in systems]...) + D22 = blockdiag([s.D22 for s in systems]...) + + sysnew = StateSpace(A, [B1 B2], [C1; C2], [D11 D12; D21 D22], Ts) + return PartionedStateSpace(sysnew, sum(s -> s.nu1, systems), ny1) +end diff --git a/src/types/StateSpace.jl b/src/types/StateSpace.jl index 8d7ecdbc7..d58c14241 100644 --- a/src/types/StateSpace.jl +++ b/src/types/StateSpace.jl @@ -158,7 +158,6 @@ Base.:^(sys::StateSpace, p::Integer) = Base.power_by_squaring(sys, p) ##################################################################### Base.ndims(::StateSpace) = 2 # NOTE: Also for SISO systems? Base.size(sys::StateSpace) = (noutputs(sys), ninputs(sys)) # NOTE: or just size(get_D(sys)) -Base.size(sys::StateSpace, d) = d <= 2 ? size(sys)[d] : 1 Base.eltype(::Type{S}) where {S<:StateSpace} = S function Base.getindex(sys::StateSpace, inds...) diff --git a/src/types/TransferFunction.jl b/src/types/TransferFunction.jl index a7a94d0f7..ba8d43025 100644 --- a/src/types/TransferFunction.jl +++ b/src/types/TransferFunction.jl @@ -44,7 +44,6 @@ ninputs(G::TransferFunction) = size(G.matrix, 2) ## INDEXING ## Base.ndims(::TransferFunction) = 2 Base.size(G::TransferFunction) = size(G.matrix) -Base.size(G::TransferFunction, d) = size(G.matrix, d) Base.eltype(::Type{S}) where {S<:TransferFunction} = S function Base.getindex(G::TransferFunction{S}, inds...) where {S<:SisoTf} diff --git a/src/types/conversion.jl b/src/types/conversion.jl index 6bd2dbd84..cdcfea5d9 100644 --- a/src/types/conversion.jl +++ b/src/types/conversion.jl @@ -49,16 +49,24 @@ Base.convert(::Type{StateSpace{T, MT}}, b::Number) where {T, MT} = convert(State # end # -function convert(::Type{TransferFunction{S}}, G::TransferFunction) where S - Gnew_matrix = Matrix{S}(undef, size(G)) - for i in eachindex(G.matrix) - Gnew_matrix[i] = convert(S, G.matrix[i]) +function convert(::Type{S}, G::TransferFunction) where {SisoT, S<:TransferFunction{SisoT}} + if G isa S + return G + else + Gnew_matrix = Matrix{SisoT}(undef, size(G)) + for i in eachindex(G.matrix) + Gnew_matrix[i] = convert(SisoT, G.matrix[i]) + end + return TransferFunction{SisoT}(Gnew_matrix, G.Ts) end - return TransferFunction{S}(Gnew_matrix, G.Ts) end -function convert(::Type{StateSpace{T,MT}}, sys::StateSpace) where {T, MT} - return StateSpace{T,MT}(convert(MT, sys.A), convert(MT, sys.B), convert(MT, sys.C), convert(MT, sys.D), sys.Ts) +function convert(::Type{S}, sys::StateSpace) where {T, MT, S <:StateSpace{T,MT}} + if sys isa S + return sys + else + return StateSpace{T,MT}(convert(MT, sys.A), convert(MT, sys.B), convert(MT, sys.C), convert(MT, sys.D), sys.Ts) + end end function Base.convert(::Type{StateSpace}, G::TransferFunction{<:SisoTf{T0}}) where {T0<:Number} diff --git a/src/types/promotion.jl b/src/types/promotion.jl index 37fec014e..87c077bbd 100644 --- a/src/types/promotion.jl +++ b/src/types/promotion.jl @@ -33,6 +33,13 @@ function Base.promote_rule(::Type{TransferFunction{S1}}, ::Type{StateSpace{T2,MT end # NOTE: Perhaps should try to keep matrix structure? +function Base.promote_rule(::Type{TransferFunction{S1}}, ::Type{DelayLtiSystem{T2}}) where {T1,S1<:SisoTf{T1},T2} + DelayLtiSystem{promote_type(T1,T2)} +end +function Base.promote_rule(::Type{StateSpace{T1,MT}}, ::Type{DelayLtiSystem{T2}}) where {T1,MT,T2} + DelayLtiSystem{promote_type(T1,T2)} +end + Base.promote_rule(::Type{TransferFunction{S1}}, ::Type{TransferFunction{S2}}) where {S1, S2} = TransferFunction{promote_type(S1, S2)} #Base.promote_rule(::Type{SisoTf}, ::Type{TransferFunction}) = TransferFunction #Base.promote_rule(::Type{SisoZpk}, ::Type{TransferFunction}) = TransferFunction @@ -62,6 +69,14 @@ Base.promote_rule(::Type{TransferFunction{SisoZpk{T1,TR1}}}, ::Type{M2}) where { Base.promote_rule(::Type{TransferFunction{SisoRational{T1}}}, ::Type{M2}) where {T1, T2, M2<:AbstractMatrix{T2}} = TransferFunction{SisoRational{promote_type(T1, T2)}} +function Base.promote_rule(::Type{StateSpace{T1, MT1}}, ::Type{MT2}) where {T1, MT1, MT2<:AbstractMatrix} + MT = promote_type(MT1, MT2) + StateSpace{eltype(MT), MT} +end + +Base.promote_rule(::Type{DelayLtiSystem{T1}}, ::Type{MT1}) where {T1, MT1<:AbstractMatrix} = + DelayLtiSystem{promote_type(T1, eltype(MT1))} + #Base.promote_rule{S<:TransferFunction{<:SisoTf}}(::Type{S}, ::Type{<:Real}) = S # We want this, but not possible, so hardcode for SisoTypes diff --git a/test/test_delayed_systems.jl b/test/test_delayed_systems.jl index 54422e0d2..3a41e1b98 100644 --- a/test/test_delayed_systems.jl +++ b/test/test_delayed_systems.jl @@ -5,7 +5,7 @@ @test typeof(promote(delay(0.2), ss(1.0 + im))[1]) == DelayLtiSystem{Complex{Float64}} # Extremely baseic tests -@test freqresp(delay(1), ω)[:] ≈ exp.(-im*ω) rtol=1e-15 +@test freqresp(delay(1), ω) ≈ reshape(exp.(-im*ω), length(ω), 1, 1) rtol=1e-15 @test freqresp(delay(2.5), ω)[:] ≈ exp.(-2.5im*ω) rtol=1e-15 @test freqresp(3.5*delay(2.5), ω)[:] ≈ 3.5*exp.(-2.5im*ω) rtol=1e-15 @test freqresp(delay(2.5)*1.5, ω)[:] ≈ exp.(-2.5im*ω)*1.5 rtol=1e-15 @@ -22,10 +22,11 @@ P2_fr = (im*ω .+ 1) ./ (im*ω .+ 2) ## Addition -@test_broken freqresp(1 + delay(1), ω)[:] ≈ 1 .+ exp.(-im*ω) # FIXME; Add suitable conversion from Int64 to DelayLtiSystem +@test freqresp(1 + delay(1), ω)[:] ≈ 1 .+ exp.(-im*ω) # FIXME; Add suitable conversion from Int64 to DelayLtiSystem @test freqresp(P1 + delay(1), ω)[:] ≈ P1_fr .+ exp.(-im*ω) -#FIXME: the following gives a crash.. freqresp(P1 - delay(1), ω)[:] ≈ P1_fr .+ exp.(-im*ω) +# Substraction +@test freqresp(P1 - delay(1), ω)[:] ≈ P1_fr .- exp.(-im*ω) ## Multiplication @@ -50,14 +51,14 @@ P2_fr = (im*ω .+ 1) ./ (im*ω .+ 2) @test freqresp(feedback(delay(2.0), 0.5), ω) ≈ exp.(-2im*ω) ./ (1 .+ 0.5*exp.(-2im*ω)) @test freqresp(feedback(P1, delay(1)), ω)[:] ≈ P1_fr ./ (1 .+ exp.(-im*ω) .* P1_fr) rtol=1e-15 -@test freqresp(feedback(delay(1), P1), ω)[:] ≈ exp.(-im*ω) ./ (1 .+ exp.(-im*ω) .* P1_fr) #FIXME: Answer is Inf, but should give error.. rtol=1e-15 +@test freqresp(feedback(delay(1), P1), ω)[:] ≈ exp.(-im*ω) ./ (1 .+ exp.(-im*ω) .* P1_fr) rtol=1e-15 #FIXME: Answer is Inf, but should give error.. rtol=1e-15 @test freqresp(feedback(P1*delay(1), 1.0), ω)[:] ≈ P1_fr .* exp.(-im*ω) ./ (1 .+ exp.(-im*ω) .* P1_fr) rtol=1e-15 @test freqresp(feedback(1.0, P1*delay(1)), ω)[:] ≈ 1 ./ (1 .+ exp.(-im*ω) .* P1_fr) rtol=1e-15 @test freqresp(feedback(1.0, P2*0.5*(ss(1.0) + delay(2))), ω)[:] ≈ 1 ./(1 .+ P2_fr .* 0.5.*(1 .+ exp.(-2*im*ω))) -@test_broken freqresp(1.0 + delay(2), ω)[:] # TODO: Add addition for DelayLtiSystem. how to do this in a convenient ammner? +@test freqresp(1.0 + delay(2), ω)[:] ≈ 1 .+ exp.(-2im*ω) rtol=1e-15 G = ss(0.5) + 0.5*delay(2)# + 0.5*delay(3) G_fr = 0.5 .+ 0.5*exp.(-2*im*ω)# + 0.5*exp.(-3*im*ω) @@ -68,6 +69,11 @@ G_fr = 0.5 .+ 0.5*exp.(-2*im*ω)# + 0.5*exp.(-3*im*ω) @test freqresp(feedback(P2, G), ω)[:] ≈ P2_fr ./(1 .+ P2_fr .* G_fr) rtol=1e-15 +# Random conversions +sys1 = DelayLtiSystem(1.0/s) +@test sys1.P.A == sys1.P.D == fill(0,1,1) +@test sys1.P.B*sys1.P.C == fill(1,1,1) +@test sys1.Tau == [] ## Multiple Delays G = ss(1.0) + delay(2) + delay(3) @@ -83,4 +89,44 @@ expected_sys_fr = 1.0 ./ (P1_fr .* (exp.(-2*im*ω) + exp.(-3*im*ω)) .+ 1) @test freqresp(feedback(1.0*P1, G*G*P2*P2), ω)[:] ≈ P1_fr ./(1 .+ (G_fr.^2).*P1_fr.*P2_fr.^2) rtol=1e-15# Somewhat pathological system though +s = tf("s") +s11 = feedback(ss(1/s), delay(1)) +s12 = ss(1/s) +s21 = DelayLtiSystem(ss(1/(s+1))) +s22 = ss(1/(s+10)) + +s1 = [s11 s12] +s2 = [s21 s22] + +f1 = [s1;s2] +f2 = [s11 s12; + s21 s22] + +# Test that different consatenations work +w = 10 .^ (-2:0.1:2) +@test freqresp(f1, w) ≈ freqresp(f2, w) rtol=1e-15 + +# This used to be a weird bug +@test freqresp(s11, w) ≈ freqresp(f2[1,1], w) rtol=1e-15 + + #FIXME: A lot more tests, including MIMO systems in particular + +# Test step +y1, t1, x1 = step([s11;s12], 10) +@test y1[:,2] ≈ step(s12, t1)[1] rtol = 1e-14 + +t = 0.0:0.1:10 +y2, t2, x2 = step(s1, t) +# TODO Figure out which is inexact here +@test y2[:,1,1:1] + y2[:,1,2:2] ≈ step(s11, t)[1] + step(s12, t)[1] rtol=1e-5 + +y3, t3, x3 = step([s11; s12], t) +@test y3[:,1,1] ≈ step(s11, t)[1] rtol = 1e-4 +@test y3[:,2,1] ≈ step(s12, t)[1] rtol = 1e-14 + +y1, t1, x1 = step(DelayLtiSystem([1.0/s 2/s; 3/s 4/s]), t) +y2, t2, x2 = step([1.0/s 2/s; 3/s 4/s], t) +@test y1 ≈ y2 rtol=1e-15 +@test size(x1,1) == length(t) +@test size(x1,3) == 2 From eba8e5aefd3daf95ee4b6358d6cc3496c0f7e09b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mattias=20F=C3=A4lt?= Date: Sat, 2 Feb 2019 16:08:26 +0100 Subject: [PATCH 10/30] Minor bug in test, and removed prints --- src/connections.jl | 3 --- src/delay_systems.jl | 4 ---- src/types/PartionedStateSpace.jl | 8 +------- test/test_delayed_systems.jl | 3 +-- 4 files changed, 2 insertions(+), 16 deletions(-) diff --git a/src/connections.jl b/src/connections.jl index e04515747..5616dab20 100644 --- a/src/connections.jl +++ b/src/connections.jl @@ -46,9 +46,6 @@ append(systems::LTISystem...) = append(promote(systems...)...) # TODO Move size check to wrappers function Base.vcat(systems::DelayLtiSystem...) - for sys in systems - println(sys.P.ny1) - end P = vcat_1([sys.P for sys in systems]...) # See PartitionedStateSpace Tau = vcat([sys.Tau for sys in systems]...) return DelayLtiSystem(P, Tau) diff --git a/src/delay_systems.jl b/src/delay_systems.jl index 5a4a5d609..64eff4c65 100644 --- a/src/delay_systems.jl +++ b/src/delay_systems.jl @@ -50,13 +50,10 @@ end function lsim(sys::DelayLtiSystem{T}, u, t::AbstractArray{<:T}; x0=fill(zero(T), nstates(sys)), alg=MethodOfSteps(Tsit5())) where T # Make u! in-place function of u u! = if isa(u, Number) || isa(u,AbstractVector) # Allow for u to be a constant number or vector - println("Number vector") (uout, t) -> uout .= u elseif DiffEqBase.isinplace(u, 2) # If u is an inplace (more than 1 argument function) - println("Inplace") u else # If u is a regular u(t) function - println("Outplace") (out, t) -> (out .= u(t)) end _lsim(sys, UWrapper(u!), t, x0, alg) @@ -110,7 +107,6 @@ function _lsim(sys::DelayLtiSystem{T}, u!, t::AbstractArray{<:T}, x0::Vector{T}, sol = DelayDiffEq.solve(prob, alg, saveat=t) x = sol.u::Array{Array{T,1},1} # the states are labeled u in DelayDiffEq - println(size(x)) y = Array{T,2}(undef, noutputs(sys), length(t)) d = Array{T,2}(undef, size(C2,1), length(t)) diff --git a/src/types/PartionedStateSpace.jl b/src/types/PartionedStateSpace.jl index 5a3c63589..67c76b167 100644 --- a/src/types/PartionedStateSpace.jl +++ b/src/types/PartionedStateSpace.jl @@ -150,8 +150,6 @@ end """ function vcat_1(systems::PartionedStateSpace...) # Perform checks - println("vcal $(length(systems))") - println(systems) Ts = systems[1].P.Ts if !all(s.P.Ts == Ts for s in systems) error("All systems have same sample time") @@ -165,10 +163,7 @@ function vcat_1(systems::PartionedStateSpace...) B1 = vcat([s.B1 for s in systems]...) B2 = blockdiag([s.B2 for s in systems]...) - for i = 1:length(systems) - println(systems[i].ny1) - end - println("after") + C1 = blockdiag([s.C1 for s in systems]...) C2 = blockdiag([s.C2 for s in systems]...) @@ -190,7 +185,6 @@ end for u1_i, u2_i, y1_i, y2_i, where i denotes system i """ function hcat_1(systems::PartionedStateSpace...) - println("hcat $(length(systems))") # Perform checks Ts = systems[1].P.Ts if !all(s.P.Ts == Ts for s in systems) diff --git a/test/test_delayed_systems.jl b/test/test_delayed_systems.jl index 3a41e1b98..735bf1025 100644 --- a/test/test_delayed_systems.jl +++ b/test/test_delayed_systems.jl @@ -69,6 +69,7 @@ G_fr = 0.5 .+ 0.5*exp.(-2*im*ω)# + 0.5*exp.(-3*im*ω) @test freqresp(feedback(P2, G), ω)[:] ≈ P2_fr ./(1 .+ P2_fr .* G_fr) rtol=1e-15 +s = tf("s") # Random conversions sys1 = DelayLtiSystem(1.0/s) @test sys1.P.A == sys1.P.D == fill(0,1,1) @@ -88,8 +89,6 @@ expected_sys_fr = 1.0 ./ (P1_fr .* (exp.(-2*im*ω) + exp.(-3*im*ω)) .+ 1) @test freqresp(feedback(1.0*P1, G*G*P2*P2), ω)[:] ≈ P1_fr ./(1 .+ (G_fr.^2).*P1_fr.*P2_fr.^2) rtol=1e-15# Somewhat pathological system though - -s = tf("s") s11 = feedback(ss(1/s), delay(1)) s12 = ss(1/s) s21 = DelayLtiSystem(ss(1/(s+1))) From 1a785d0b030dc53006dce51533eba3f111733962 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mattias=20F=C3=A4lt?= Date: Mon, 25 Mar 2019 20:09:28 +0100 Subject: [PATCH 11/30] Added impulse to delay lsim --- src/delay_systems.jl | 20 ++++++++++++++++ src/types/DelayLtiSystem.jl | 2 +- src/types/PartionedStateSpace.jl | 12 +++++----- test/test_delayed_systems.jl | 41 ++++++++++++++++++++++++++++++++ 4 files changed, 68 insertions(+), 7 deletions(-) diff --git a/src/delay_systems.jl b/src/delay_systems.jl index 64eff4c65..9d89960a3 100644 --- a/src/delay_systems.jl +++ b/src/delay_systems.jl @@ -78,6 +78,7 @@ function dde_param(dx, x, h!, p, t) return end +# TODO Discontinuities in u are not handled well yet. function _lsim(sys::DelayLtiSystem{T}, u!, t::AbstractArray{<:T}, x0::Vector{T}, alg) where T P = sys.P @@ -190,3 +191,22 @@ function Base.step(sys::DelayLtiSystem{T}, t::AbstractVector, kwargs...) where T end return y, tout, x end + + +function impulse(sys::DelayLtiSystem{T}, t::AbstractVector, kwargs...) where T + nu = ninputs(sys) + if t[1] != 0 + throw(ArgumentError("First time point must be 0 in impulse")) + end + u = (out, t) -> (out .= 0) + if nu == 1 + y, tout, x = lsim(sys, u, t, x0=sys.P.B[:,1], kwargs...) + else + x = Array{T}(undef, length(t), nstates(sys), nu) + y = Array{T}(undef, length(t), noutputs(sys), nu) + for i=1:nu + y[:,:,i], tout, x[:,:,i] = lsim(sys[:,i], u, t, x0=sys.P.B[:,i], kwargs...) + end + end + return y, tout, x +end diff --git a/src/types/DelayLtiSystem.jl b/src/types/DelayLtiSystem.jl index aba3880f6..b0ef736a1 100644 --- a/src/types/DelayLtiSystem.jl +++ b/src/types/DelayLtiSystem.jl @@ -18,7 +18,7 @@ function DelayLtiSystem{T}(sys::StateSpace, Tau::Vector{Float64}) where T<:Numbe ny = noutputs(sys) - length(Tau) if nu < 0 || ny < 0 - error("Time vector is too long.") + throw(ArgumentError("Time vector is too long.")) end psys = PartionedStateSpace{StateSpace{T,Matrix{T}}}(sys, nu, ny) diff --git a/src/types/PartionedStateSpace.jl b/src/types/PartionedStateSpace.jl index 67c76b167..e6377193d 100644 --- a/src/types/PartionedStateSpace.jl +++ b/src/types/PartionedStateSpace.jl @@ -161,15 +161,15 @@ function vcat_1(systems::PartionedStateSpace...) A = blockdiag([s.A for s in systems]...) - B1 = vcat([s.B1 for s in systems]...) + B1 = reduce(vcat, [s.B1 for s in systems]) B2 = blockdiag([s.B2 for s in systems]...) C1 = blockdiag([s.C1 for s in systems]...) C2 = blockdiag([s.C2 for s in systems]...) - D11 = vcat([s.D11 for s in systems]...) + D11 = reduce(vcat, [s.D11 for s in systems]) D12 = blockdiag([s.D12 for s in systems]...) - D21 = vcat([s.D21 for s in systems]...) + D21 = reduce(vcat, [s.D21 for s in systems]) D22 = blockdiag([s.D22 for s in systems]...) sysnew = StateSpace(A, [B1 B2], [C1; C2], [D11 D12; D21 D22], Ts) @@ -200,11 +200,11 @@ function hcat_1(systems::PartionedStateSpace...) B1 = blockdiag([s.B1 for s in systems]...) B2 = blockdiag([s.B2 for s in systems]...) - C1 = hcat([s.C1 for s in systems]...) + C1 = reduce(hcat, [s.C1 for s in systems]) C2 = blockdiag([s.C2 for s in systems]...) - D11 = hcat([s.D11 for s in systems]...) - D12 = hcat([s.D12 for s in systems]...) + D11 = reduce(hcat, [s.D11 for s in systems]) + D12 = reduce(hcat, [s.D12 for s in systems]) D21 = blockdiag([s.D21 for s in systems]...) D22 = blockdiag([s.D22 for s in systems]...) diff --git a/test/test_delayed_systems.jl b/test/test_delayed_systems.jl index 735bf1025..0f733f1e7 100644 --- a/test/test_delayed_systems.jl +++ b/test/test_delayed_systems.jl @@ -129,3 +129,44 @@ y2, t2, x2 = step([1.0/s 2/s; 3/s 4/s], t) @test y1 ≈ y2 rtol=1e-15 @test size(x1,1) == length(t) @test size(x1,3) == 2 + + +##################### Test with known solution +K = 2.0 +sys_known = feedback(delay(1)*K/s, 1) + +ystep, t, _ = step(sys_known, 3) + +function y_expected(t, K) + if t < 1 + return 0 + elseif t < 2 + return K*(t-1) + elseif t <= 3 + return K*(t-1)-1/2*K^2*(t-2)^2 + else + throw(ArgumentError("Test not defined here")) + end +end + +@test ystep ≈ y_expected.(t, K) atol = 1e-7 + +function dy_expected(t, K) + if t < 1 + return 0 + elseif t < 2 + return K + elseif t <= 3 + return K - K^2*(t-2) + else + throw(ArgumentError("Test not defined here")) + end +end + +y_impulse, t, _ = impulse(sys_known, 3) + +# TODO Better accuracy for impulse +@test y_impulse ≈ dy_expected.(t, K) rtol=1e-2 +@test maximum(abs, y_impulse - dy_expected.(t, K)) < 1e-2 + +@time [s11; s12] From 12ce306dca1365527f70454dc2db97441bf66651 Mon Sep 17 00:00:00 2001 From: olof3 Date: Tue, 22 Jan 2019 10:03:23 +0100 Subject: [PATCH 12/30] WIP: Delayed LTI systems. --- example/delayed_lti_system.jl | 19 ++++ src/ControlSystems.jl | 9 ++ src/delay_systems.jl | 26 +++++ src/types/DelayLtiSystem.jl | 72 +++++++++++++ src/types/PartionedStateSpace.jl | 160 ++++++++++++++++++++++++++++ test/test_delayed_systems.jl | 44 ++++++++ test/test_partitioned_statespace.jl | 41 +++++++ 7 files changed, 371 insertions(+) create mode 100644 example/delayed_lti_system.jl create mode 100644 src/delay_systems.jl create mode 100644 src/types/DelayLtiSystem.jl create mode 100644 src/types/PartionedStateSpace.jl create mode 100644 test/test_delayed_systems.jl create mode 100644 test/test_partitioned_statespace.jl diff --git a/example/delayed_lti_system.jl b/example/delayed_lti_system.jl new file mode 100644 index 000000000..df6a68296 --- /dev/null +++ b/example/delayed_lti_system.jl @@ -0,0 +1,19 @@ +using Plots + +# Frequency domain analysis of a simple system with time delays + +s = tf("s") + +P = delay(0.2) * DelayLtiSystem(ss(1/(s+1))) + +K = 3; Ti = 0.3; +C = DelayLtiSystem(ss(K*(1 + 1/s))) + +ω = exp10.(LinRange(-2,2,500)) + +L_fr = freqresp(C*P, ω)[:] +plot(real(L_fr), imag(L_fr), xlim=[-2,1], ylim=[-2,2], title="Nyquist curve") + +G_yd = feedback(P, C) +plot(ω, abs.(freqresp(G_yd, ω)[:]), xscale=:log, yscale=:log, + title="Transfer function from load disturbances to output.") diff --git a/src/ControlSystems.jl b/src/ControlSystems.jl index 76210ace5..61553806b 100644 --- a/src/ControlSystems.jl +++ b/src/ControlSystems.jl @@ -5,6 +5,7 @@ export LTISystem, StateSpace, HeteroStateSpace, TransferFunction, + DelayLtiSystem, ss, tf, zpk, @@ -68,6 +69,8 @@ export LTISystem, bode, nyquist, sigma, + # delay systems + delay, # utilities num, #Deprecated den, #Deprecated @@ -81,6 +84,7 @@ export LTISystem, using Polynomials, OrdinaryDiffEq, Plots, LaTeXStrings, LinearAlgebra export Plots import Base: +, -, *, /, (==), (!=), isapprox, convert, promote_op +import Base: getproperty import LinearAlgebra: BlasFloat export lyap # Make sure LinearAlgebra.lyap is available import Printf, Colors @@ -102,6 +106,9 @@ include("types/SisoTfTypes/conversion.jl") include("types/StateSpace.jl") +include("types/PartionedStateSpace.jl") +include("types/DelayLtiSystem.jl") + # Convenience constructors include("types/tf.jl") include("types/zpk.jl") @@ -128,6 +135,8 @@ include("synthesis.jl") include("simulators.jl") include("pid_design.jl") +include("delay_systems.jl") + include("plotting.jl") @deprecate num numvec diff --git a/src/delay_systems.jl b/src/delay_systems.jl new file mode 100644 index 000000000..aa3c0ae34 --- /dev/null +++ b/src/delay_systems.jl @@ -0,0 +1,26 @@ +function freqresp(sys::DelayLtiSystem, ω::AbstractVector{T}) where {T <: Real} + ny = noutputs(sys) + nu = ninputs(sys) + + P_fr = ControlSystems.freqresp(sys.P, ω); + + println(ω) + + # FIXME: Different dimensions compared to standard freqresp + 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] + + # FIXME: when there is no delays... + + delay_vect_fr = Base.exp.(im*sys.Tau*ω[ω_idx]) # Frequency response of the block diagonal matrix + + G_fr[:,:,ω_idx] .= P11_fr + P12_fr/(Diagonal(delay_vect_fr) - P22_fr)*P21_fr # The matrix is invertible (?!) + end + + return G_fr +end diff --git a/src/types/DelayLtiSystem.jl b/src/types/DelayLtiSystem.jl new file mode 100644 index 000000000..101d91b5d --- /dev/null +++ b/src/types/DelayLtiSystem.jl @@ -0,0 +1,72 @@ +struct DelayLtiSystem{T} <: LTISystem + P::StateSpace{T,Matrix{T}} + Tau::Vector{Float64} # The length of the vector tau implicitly defines the partitionging of P + + # function DelayLtiSystem(P::StateSpace{T, MT}, Tau::Vector{Float64}) + # if ControlSystems.noutputs(P) < length(Tau) || + # ControlSystems.noutputs(P) < length(Tau) + # error("Length of time-vector is too long given the size of the partitioned system P.") + # end + # new{T}(P, Tau) + # end +end + +DelayLtiSystem(sys::StateSpace) = DelayLtiSystem(sys, Float64[]) +#Base.convert(::Type{S}, ) where {S<:DelayLtiSystem} = # Need to handle numerical arguments + +Base.promote_rule(::Type{<:StateSpace}, ::Type{S}) where {S<:DelayLtiSystem} = S + +ninputs(sys::DelayLtiSystem) = size(sys.P)[2] - length(sys.Tau) +noutputs(sys::DelayLtiSystem) = size(sys.P)[1] - length(sys.Tau) + + +function +(sys1::DelayLtiSystem, sys2::DelayLtiSystem) + s1 = PartionedStateSpace(sys1.P, ninputs(sys1), noutputs(sys1)) + s2 = PartionedStateSpace(sys2.P, ninputs(sys2), noutputs(sys2)) + + s_new = s1 + s2 + Tau_new = [sys1.Tau; sys2.Tau] + + DelayLtiSystem(s_new.P, Tau_new) +end + + +function *(sys1::DelayLtiSystem, sys2::DelayLtiSystem) + s1 = PartionedStateSpace(sys1.P, ninputs(sys1), noutputs(sys1)) + s2 = PartionedStateSpace(sys2.P, ninputs(sys2), noutputs(sys2)) + + s_new = s1 * s2 + Tau_new = [sys1.Tau; sys2.Tau] + + DelayLtiSystem(s_new.P, Tau_new) +end + + +function feedback(sys1::DelayLtiSystem, sys2::DelayLtiSystem) + s1 = PartionedStateSpace(sys1.P, ninputs(sys1), noutputs(sys1)) + s2 = PartionedStateSpace(sys2.P, ninputs(sys2), noutputs(sys2)) + + s_new = feedback(s1, s2) + Tau_new = [sys1.Tau; sys2.Tau] + + DelayLtiSystem(s_new.P, Tau_new) +end + +function delay(tau::Real, T::Type{<:Number}=Float64) + return DelayLtiSystem(ControlSystems.ss([zero(T) one(T); one(T) zero(T)]), [float(tau)]) +end + +# function exp(G::TransferFunction) +# if (size(G.matrix) != [1, 1]) || ~isone(G.matrix[1].den) || length(G.matrix[1].num) >= 2 +# error("exp only accepts TransferFunction arguemns of the form (a*s + b)") +# end +# +# a = G.matrix[1].num[1] +# b = G.matrix[1].num[0] +# +# if a > 0 +# error("Delay needs to be causal.") +# end +# +# return exp(b) * delay(a) +# end diff --git a/src/types/PartionedStateSpace.jl b/src/types/PartionedStateSpace.jl new file mode 100644 index 000000000..8908ae358 --- /dev/null +++ b/src/types/PartionedStateSpace.jl @@ -0,0 +1,160 @@ +""" +A StateSpace model with a partioning imposed according to + +A | B1 B2 +——— ———————— +C1 | D11 D12 +C2 | D21 D22 + +It corresponds to partioned input and output signals +u = [u1 u2]^T +y = [y1 y2]^T + +""" +struct PartionedStateSpace{S} + P::S + nu1::Int + ny1::Int +end + + +function getproperty(sys::PartionedStateSpace, d::Symbol) + P = getfield(sys, :P) + nu1 = getfield(sys, :nu1) + ny1 = getfield(sys, :ny1) + + if d == :P + return P + elseif d == :nu1 + return nu1 + elseif d == :ny1 + return ny1 + elseif d == :A + return P.A + elseif d == :B1 + return P.B[:, 1:nu1] + elseif d == :B2 + return P.B[:, nu1+1:end] + elseif d == :C1 + return P.C[1:ny1, :] + elseif d == :C2 + return P.C[ny1+1:end, :] + elseif d == :D11 + return P.D[1:ny1, 1:nu1] + elseif d == :D12 + return P.D[1:ny1, nu1+1:end] + elseif d == :D21 + return P.D[ny1+1:end, 1:nu1] + elseif d == :D22 + return P.D[ny1+1:end, nu1+1:end] + else + return getfield(P, d) + end +end + + +# There should already exist a function like this somewhare? +function blkdiag(A1::Matrix{T1}, A2::Matrix{T2}) where {T1<:Number, T2<:Number} + T = promote_type(T1, T2) + + dims1 = size(A1) + dims2 = size(A2) + + A_new = zeros(Float64, dims1 .+ dims2) + A_new[1:dims1[1], 1:dims1[2]] = A1 + A_new[dims1[1]+1:end, dims1[2]+1:end] = A2 + + return A_new +end + + +function +(s1::PartionedStateSpace, s2::PartionedStateSpace) + A = blkdiag(s1.A, s2.A) + + B = [[s1.B1; s2.B1] blkdiag(s1.B2, s2.B2)] + + C = [[s1.C1 s2.C1]; + blkdiag(s1.C2, s2.C2)] + + D = [(s1.D11 + s2.D11) s1.D12 s2.D12; + [s1.D21; s2.D21] blkdiag(s1.D22, s2.D22)] + + P = StateSpace(A, B, C, D, 0) # How to handle discrete? + PartionedStateSpace(P, s1.nu1 + s2.nu1, s1.ny1 + s2.ny1) +end + + + + + +""" + Series connection of to partioned StateSpace systems. +""" +function *(s1::PartionedStateSpace, s2::PartionedStateSpace) + println("Hej") + A = [s1.A s1.B1*s2.C1; + zeros(size(s2.A,1),size(s1.A,2)) s2.A] + + B = [s1.B1*s2.D11 s1.B2 s1.B1*s2.D12; + s2.B1 zeros(size(s2.B2,1),size(s1.B2,2)) s2.B2] + + C = [s1.C1 s1.D11*s2.C1; + s1.C2 s1.D21*s2.C1; + zeros(size(s2.C2,1),size(s1.C2,2)) s2.C2] + + D = [s1.D11*s2.D11 s1.D12 s1.D11*s2.D12; + s1.D21*s2.D11 s1.D22 s1.D21*s2.D12; + s2.D21 zeros(size(s2.D22,1),size(s1.D22,2)) s2.D22 ] + + P = StateSpace(A, B, C, D, 0) + PartionedStateSpace(P, s2.nu1, s1.ny1) +end + + + +# Algebraic loops?! + +function feedback(s1::PartionedStateSpace, s2::PartionedStateSpace) + X_11 = [-s2.D11*s1.C1 -s2.C1] + X_21 = [s1.C1 -s1.D11*s2.C1] + + # For the case of two outputs + # X_12 = [I -s2.D11 -s2.D11*s1.D12 -s2.D12] + # X_22 = [-s1.D11 I -s1.D12 s1.D11*s2.D12] + X_12 = [I -s2.D11*s1.D12 -s2.D12] + X_22 = [-s1.D11 -s1.D12 s1.D11*s2.D12] + + + + A = [s1.B1 * X_11 ; s2.B1 * X_21] + blkdiag(s1.A, s2.A) + + B = [s1.B1 * X_12 ; s2.B1 * X_22] + tmp = blkdiag(s1.B2, s2.B2) + B[:, end-size(tmp,2)+1:end] .+= tmp + + C = [s1.D11 * X_11 ; + s1.D21 * X_11 ; + s2.D21 * X_21 ] + [s1.C1 zeros(size(s1.C1,1),size(s2.C1,2)); blkdiag(s1.C2, s2.C2)] + + D = [s1.D11 * X_12 ; + s1.D21 * X_12 ; + s2.D21 * X_22 ] + tmp = [s1.D12 zeros(size(s1.D12,1),size(s2.D12,2)); blkdiag(s1.D22, s2.D22)] + D[:, end-size(tmp,2)+1:end] .+= tmp + + # in case it is desired to consider both outputs + # C = [s1.D11 * X_11 ; + # s2.D11 * X_21 ; + # s1.D21 * X_11 ; + # s2.D21 * X_21 ] + [blkdiag(s1.C1, s2.C1); blkdiag(s1.C2, s2.C2)] + # + # D = [s1.D11 * X_12 ; + # s2.D11 * X_22 ; + # s1.D21 * X_12 ; + # s2.D21 * X_22 ] + #tmp = [blkdiag(s1.D12, s2.D12); blkdiag(s1.D22, s2.D22)] + #D[:, end-size(tmp,2)+1:end] .+= tmp + + P = StateSpace(A, B, C, D, 0) + PartionedStateSpace(P, s2.nu1, s1.ny1) +end diff --git a/test/test_delayed_systems.jl b/test/test_delayed_systems.jl new file mode 100644 index 000000000..c30a9a818 --- /dev/null +++ b/test/test_delayed_systems.jl @@ -0,0 +1,44 @@ +ω = 0.0:8 + +freqresp(delay(1), ω)[:] ≈ exp.(-im*ω) +freqresp(delay(2.5), ω)[:] ≈ exp.(-2.5*im*ω) + +P1 = DelayLtiSystem(ss(-1.0, 1, 1, 0)) +P1_fr ≈ 1 ./ (im*ω .+ 1) + +P2 = DelayLtiSystem(ss(-2.0, -1, 1, 1)) # (s+1)/(s+2) +P2_fr = (im*ω .+ 1) ./ (im*ω .+ 2) + + +freqresp(P1 * delay(1), ω)[:] ≈ P1_fr .* exp.(-im*ω) +freqresp(delay(1) * P1, ω)[:] ≈ P1_fr .* exp.(-im*ω) + +freqresp(P2 * delay(1), ω)[:] ≈ P2_fr .* exp.(-im*ω) +freqresp(delay(1) * P2, ω)[:] ≈ P2_fr .* exp.(-im*ω) + + +# feedback +freqresp(feedback(P1, delay(1)), ω)[:] ≈ P1_fr ./ (1 .+ exp.(-im*ω) .* P1_fr) +freqresp(feedback(delay(1), P1), ω)[:] ≈ exp.(-im*ω) ./ (1 .+ exp.(-im*ω) .* P1_fr) + +G_fr = freqresp(feedback(P1*delay(1), DelayLtiSystem(ss(1.0))), ω)[:] +G_fr ≈ P1_fr .* exp.(-im*ω) ./ (1 .+ exp.(-im*ω) .* P1_fr) + + +# Test DelayLtiSystem objects without delays +freqresp(feedback(P1, P1), ω)[:] ≈ P1_fr ./ (1 .+ P1_fr .* P1_fr) + + +# FIXME: Automatic conversion to DelayLtiSystem + + +nvert(DelayLtiSystem, P1) + +promote_rule(typeof(P1), typeof(P2)) +x1, x2 = promote(P1, P2) + + +# What ordering for freqresp ?! +freqresp(P.P, ω) + +G_fr = freqresp(G, ω)[:] diff --git a/test/test_partitioned_statespace.jl b/test/test_partitioned_statespace.jl new file mode 100644 index 000000000..cee4a5034 --- /dev/null +++ b/test/test_partitioned_statespace.jl @@ -0,0 +1,41 @@ +using ControlSystems +using LinearAlgebra + +## +# system of the form +# 1 | 2 3 +# ——————— +# 4 | 5 6 +# 7 | 8 9 +sys1 = ControlSystems.PartionedStateSpace(ss(1.0, [2.0 3.0], [4.0; 7.0], [5.0 6.0; 8.0 9.0]), 1, 1) + +matrix(x::Number) = fill(x, 1, 1) + +sys1.A == matrix(1.0) +sys1.B1 == matrix(2.0) +sys1.B2 == matrix(3.0) +sys1.C1 == matrix(4.0) +sys1.C2 == matrix(7.0) +sys1.D11 == matrix(5.0) +sys1.D12 == matrix(6.0) +sys1.D21 == matrix(8.0) +sys1.D22 == matrix(9.0) + + +## +sys1 = ControlSystems.PartionedStateSpace(ss(fill(1.0, 2, 2), fill(2.0, 2, 5), fill(3.0, 7, 2), fill(4.0, 7, 5)), 2, 3) + +sys1.A == fill(1.0, 2, 2) +sys1.B1 == fill(2.0, 2, 2) +sys1.B2 == fill(2.0, 2, 3) +sys1.C1 == fill(3.0, 3, 2) +sys1.C2 == fill(3.0, 4, 2) +sys1.D11 == fill(4.0, 3, 2) +sys1.D12 == fill(4.0, 3, 3) +sys1.D21 == fill(4.0, 4, 2) +sys1.D22 == fill(4.0, 4, 3) + + +# Simple test +(sys1 + sys1).P[1, 1] == (sys1.P[1,1] + sys1.P[1,1]) +(sys1 * sys1).P[1, 1] == (sys1.P[1,1] * sys1.P[1,1]) From 45d0a3c94d32dbe98e0a2fc34633d9778868e34e Mon Sep 17 00:00:00 2001 From: olof3 Date: Wed, 23 Jan 2019 11:28:35 +0100 Subject: [PATCH 13/30] Delayed LTI updates. --- example/delayed_lti_system.jl | 2 +- example/delayed_lti_timeresp.jl | 41 ++++++++++++++++++++++++++++++ src/delay_systems.jl | 11 +++----- src/types/DelayLtiSystem.jl | 20 +++++++++++---- src/types/Lti.jl | 6 +++++ src/types/PartionedStateSpace.jl | 9 +++---- test/test_delayed_systems.jl | 43 +++++++++++++++----------------- 7 files changed, 90 insertions(+), 42 deletions(-) create mode 100644 example/delayed_lti_timeresp.jl diff --git a/example/delayed_lti_system.jl b/example/delayed_lti_system.jl index df6a68296..0587d01a4 100644 --- a/example/delayed_lti_system.jl +++ b/example/delayed_lti_system.jl @@ -4,7 +4,7 @@ using Plots s = tf("s") -P = delay(0.2) * DelayLtiSystem(ss(1/(s+1))) +P = delay(0.2) * ss(1/(s+1)) K = 3; Ti = 0.3; C = DelayLtiSystem(ss(K*(1 + 1/s))) diff --git a/example/delayed_lti_timeresp.jl b/example/delayed_lti_timeresp.jl new file mode 100644 index 000000000..eea308ccc --- /dev/null +++ b/example/delayed_lti_timeresp.jl @@ -0,0 +1,41 @@ +function simulate(sys::DelayLtiSystem, tspan; u=[], x0=Float64[], alg=MethodOfSteps(Tsit5()), kwargs...) + ControlSystems.PartionedStateSpace(sys.P, 1, 1) + + nu = ControlSystems.ninputs(sys) + nx = ControlSystems.nstates(sys) + + u = (u == []) ? t -> fill(0.0, nu) : u + x0 = (x0 == []) ? fill(0.0, nx) : x0 + + # FIXME: Only works for one single time delay + # FIXME: Should error for non-zero D22 terms + dde = function (dx, x, h, p, t) + d_delayed = P.C2*h(p,t-sys.Tau[1]) + P.D21*u(t-sys.Tau[1])# + P.D22*(t-Tau[1] + dx .= P.A*x + P.B2*d_delayed + P.B1*u(t) + end + + h_initial = (p, t) -> zeros(N) + + + saved_values_y = SavedValues(Float64, Vector{Float64}) + cb = SavingCallback((x,t,integrator)->(P.C1*x + P.D11*u(t)), saved_values_y) + + prob = DDEProblem(dde, x0, h_initial, tspan, constant_lags=sys.Tau) + + sol = solve(prob, alg; saveat=0:0.02:tspan[2], callback=cb, kwargs...) + + t, x = sol.t, sol.u + + + t, x +end + +sys = feedback(1.0, ss(-1, 1, 1, 0) * delay(1.0)) + +@time t, x = simulate(sys, (0.0,4.0); u=t->[t>0 ? 1.0 : 0.0], saveat=0:0.02:5) + +x1 = [x[k][1] for k=1:length(x)] +del = [zeros(50); x1[1:end-50]] # FIXME: Not the real delayed signal... + +# The following does not work in general... just this specific problem instance +plot(t, ones(size(t)) .- x1) diff --git a/src/delay_systems.jl b/src/delay_systems.jl index aa3c0ae34..ad0e1b21d 100644 --- a/src/delay_systems.jl +++ b/src/delay_systems.jl @@ -4,10 +4,7 @@ function freqresp(sys::DelayLtiSystem, ω::AbstractVector{T}) where {T <: Real} P_fr = ControlSystems.freqresp(sys.P, ω); - println(ω) - - # FIXME: Different dimensions compared to standard freqresp - G_fr = zeros(eltype(P_fr), ny, nu, length(ω)) + G_fr = zeros(eltype(P_fr), length(ω), ny, nu) for ω_idx=1:length(ω) P11_fr = P_fr[ω_idx, 1:ny, 1:nu] @@ -15,11 +12,9 @@ function freqresp(sys::DelayLtiSystem, ω::AbstractVector{T}) where {T <: Real} P21_fr = P_fr[ω_idx, ny+1:end, 1:nu] P22_fr = P_fr[ω_idx, ny+1:end, nu+1:end] - # FIXME: when there is no delays... - - delay_vect_fr = Base.exp.(im*sys.Tau*ω[ω_idx]) # Frequency response of the block diagonal matrix + delay_matrix_fr = Diagonal(exp.(im*sys.Tau*ω[ω_idx])) # Frequency response of the diagonal matrix with delays - G_fr[:,:,ω_idx] .= P11_fr + P12_fr/(Diagonal(delay_vect_fr) - P22_fr)*P21_fr # The matrix is invertible (?!) + G_fr[ω_idx,:,:] .= P11_fr + P12_fr/(delay_matrix_fr - P22_fr)*P21_fr # The matrix is invertible (?!) end return G_fr diff --git a/src/types/DelayLtiSystem.jl b/src/types/DelayLtiSystem.jl index 101d91b5d..b6e6c4910 100644 --- a/src/types/DelayLtiSystem.jl +++ b/src/types/DelayLtiSystem.jl @@ -11,14 +11,24 @@ struct DelayLtiSystem{T} <: LTISystem # end end -DelayLtiSystem(sys::StateSpace) = DelayLtiSystem(sys, Float64[]) -#Base.convert(::Type{S}, ) where {S<:DelayLtiSystem} = # Need to handle numerical arguments +DelayLtiSystem(sys::StateSpace{T,MT}) where {T, MT} = DelayLtiSystem{T}(sys, Float64[]) +DelayLtiSystem{T}(sys::StateSpace) where T = DelayLtiSystem{T}(sys, Float64[]) -Base.promote_rule(::Type{<:StateSpace}, ::Type{S}) where {S<:DelayLtiSystem} = S -ninputs(sys::DelayLtiSystem) = size(sys.P)[2] - length(sys.Tau) -noutputs(sys::DelayLtiSystem) = size(sys.P)[1] - length(sys.Tau) +# TODO: Think through these promotions and conversions +Base.promote_rule(::Type{<:StateSpace{T1}}, ::Type{DelayLtiSystem{T2}}) where {T1<:Number,T2<:Number} = DelayLtiSystem{promote_type(T1,T2)} +Base.promote_rule(::Type{AbstractMatrix{T1}}, ::Type{DelayLtiSystem{T2}}) where {T1<:Number,T2<:Number} = DelayLtiSystem{promote_type(T1,T2)} +Base.promote_rule(::Type{T1}, ::Type{DelayLtiSystem{T2}}) where {T1<:Number,T2<:Number} = DelayLtiSystem{promote_type(T1,T2)} +#Base.promote_rule(::Type{<:UniformScaling}, ::Type{S}) where {S<:DelayLtiSystem} = DelayLtiSystem{T} +Base.convert(::Type{DelayLtiSystem{T}}, sys::StateSpace) where T = DelayLtiSystem{T}(sys) +Base.convert(::Type{DelayLtiSystem{T1}}, d::T2) where {T1,T2 <: Number} = DelayLtiSystem{T1}(ss(d)) +#Base.convert(::Type{DelayLtiSystem{T}}, sys::DelayLtiSystem) where T = DelayLtiSystem{T}(StateSpace{T,Matrix{T}}(sys)) + + +ninputs(sys::DelayLtiSystem) = ninputs(sys.P) - length(sys.Tau) +noutputs(sys::DelayLtiSystem) = noutputs(sys.P) - length(sys.Tau) +nstates(sys::DelayLtiSystem) = nstates(sys.P) function +(sys1::DelayLtiSystem, sys2::DelayLtiSystem) s1 = PartionedStateSpace(sys1.P, ninputs(sys1), noutputs(sys1)) diff --git a/src/types/Lti.jl b/src/types/Lti.jl index ace14975e..d109aac6c 100644 --- a/src/types/Lti.jl +++ b/src/types/Lti.jl @@ -3,8 +3,14 @@ abstract type LTISystem <: AbstractSystem end -(sys1::LTISystem, sys2::LTISystem) = -(promote(sys1, sys2)...) *(sys1::LTISystem, sys2::LTISystem) = *(promote(sys1, sys2)...) /(sys1::LTISystem, sys2::LTISystem) = /(promote(sys1, sys2)...) + + +feedback(sys1::Union{LTISystem,Number,AbstractMatrix{<:Number}}, + sys2::Union{LTISystem,Number,AbstractMatrix{<:Number}}) = feedback(promote(sys1, sys2)...) + Base.inv(G::LTISystem) = 1/G + """`issiso(sys)` Returns `true` if `sys` is SISO, else returns `false`.""" diff --git a/src/types/PartionedStateSpace.jl b/src/types/PartionedStateSpace.jl index 8908ae358..1901f4db6 100644 --- a/src/types/PartionedStateSpace.jl +++ b/src/types/PartionedStateSpace.jl @@ -88,10 +88,9 @@ end """ - Series connection of to partioned StateSpace systems. + Series connection of partioned StateSpace systems. """ function *(s1::PartionedStateSpace, s2::PartionedStateSpace) - println("Hej") A = [s1.A s1.B1*s2.C1; zeros(size(s2.A,1),size(s1.A,2)) s2.A] @@ -112,7 +111,7 @@ end -# Algebraic loops?! +# QUESTION: Should perhaps algebraic loops?! Perhaps issue warning if P1(∞)*P2(∞) > 1 function feedback(s1::PartionedStateSpace, s2::PartionedStateSpace) X_11 = [-s2.D11*s1.C1 -s2.C1] @@ -120,9 +119,9 @@ function feedback(s1::PartionedStateSpace, s2::PartionedStateSpace) # For the case of two outputs # X_12 = [I -s2.D11 -s2.D11*s1.D12 -s2.D12] - # X_22 = [-s1.D11 I -s1.D12 s1.D11*s2.D12] + # X_22 = [s1.D11 I -s1.D12 s1.D11*s2.D12] X_12 = [I -s2.D11*s1.D12 -s2.D12] - X_22 = [-s1.D11 -s1.D12 s1.D11*s2.D12] + X_22 = [s1.D11 -s1.D12 s1.D11*s2.D12] diff --git a/test/test_delayed_systems.jl b/test/test_delayed_systems.jl index c30a9a818..a48b6fc96 100644 --- a/test/test_delayed_systems.jl +++ b/test/test_delayed_systems.jl @@ -1,15 +1,24 @@ ω = 0.0:8 +typeof(promote(delay(0.2), ss(1))[1]) == DelayLtiSystem{Float64} +typeof(promote(delay(0.2), ss(1.0 + im))[1]) == DelayLtiSystem{Float64} + freqresp(delay(1), ω)[:] ≈ exp.(-im*ω) freqresp(delay(2.5), ω)[:] ≈ exp.(-2.5*im*ω) P1 = DelayLtiSystem(ss(-1.0, 1, 1, 0)) -P1_fr ≈ 1 ./ (im*ω .+ 1) +P1_fr = 1 ./ (im*ω .+ 1) P2 = DelayLtiSystem(ss(-2.0, -1, 1, 1)) # (s+1)/(s+2) P2_fr = (im*ω .+ 1) ./ (im*ω .+ 2) +# Addition +freqresp(P1 + delay(1), ω)[:] ≈ P1_fr .+ exp.(-im*ω) +#FIXME: the following gives a crash.. freqresp(P1 - delay(1), ω)[:] ≈ P1_fr .+ exp.(-im*ω) + + +# Multiplication freqresp(P1 * delay(1), ω)[:] ≈ P1_fr .* exp.(-im*ω) freqresp(delay(1) * P1, ω)[:] ≈ P1_fr .* exp.(-im*ω) @@ -17,28 +26,16 @@ freqresp(P2 * delay(1), ω)[:] ≈ P2_fr .* exp.(-im*ω) freqresp(delay(1) * P2, ω)[:] ≈ P2_fr .* exp.(-im*ω) -# feedback -freqresp(feedback(P1, delay(1)), ω)[:] ≈ P1_fr ./ (1 .+ exp.(-im*ω) .* P1_fr) -freqresp(feedback(delay(1), P1), ω)[:] ≈ exp.(-im*ω) ./ (1 .+ exp.(-im*ω) .* P1_fr) - -G_fr = freqresp(feedback(P1*delay(1), DelayLtiSystem(ss(1.0))), ω)[:] -G_fr ≈ P1_fr .* exp.(-im*ω) ./ (1 .+ exp.(-im*ω) .* P1_fr) - - -# Test DelayLtiSystem objects without delays +# Feedback +# The first tests don't include delays, but the linear system is of DelayLtiForm type +# (very simple system so easy to troubleshoot) +freqresp(feedback(1.0, P1), ω)[:] ≈ 1 ./ (1 .+ P1_fr) +freqresp(feedback(P1, 1.0), ω)[:] ≈ P1_fr ./ (1 .+ P1_fr) freqresp(feedback(P1, P1), ω)[:] ≈ P1_fr ./ (1 .+ P1_fr .* P1_fr) +freqresp(feedback(P1, delay(1)), ω)[:] ≈ P1_fr ./ (1 .+ exp.(-im*ω) .* P1_fr) +freqresp(feedback(delay(1), P1), ω)[:] ≈ exp.(-im*ω) ./ (1 .+ exp.(-im*ω) .* P1_fr) #FIXME: Answer is Inf, but should give error.. +freqresp(feedback(P1*delay(1), 1.0), ω)[:] ≈ P1_fr .* exp.(-im*ω) ./ (1 .+ exp.(-im*ω) .* P1_fr) +freqresp(feedback(1.0, P1*delay(1)), ω)[:] ≈ 1 ./ (1 .+ exp.(-im*ω) .* P1_fr) -# FIXME: Automatic conversion to DelayLtiSystem - - -nvert(DelayLtiSystem, P1) - -promote_rule(typeof(P1), typeof(P2)) -x1, x2 = promote(P1, P2) - - -# What ordering for freqresp ?! -freqresp(P.P, ω) - -G_fr = freqresp(G, ω)[:] +#FIXME: A lot more tests, including MIMO systems in particular From ef6ba374634c6e88d72c932a5eb80305674ca518 Mon Sep 17 00:00:00 2001 From: olof3 Date: Wed, 23 Jan 2019 17:53:05 +0100 Subject: [PATCH 14/30] Updates. --- example/delayed_lti_timeresp.jl | 31 +++++----------------- src/ControlSystems.jl | 3 ++- src/delay_systems.jl | 37 +++++++++++++++++++++++++- src/freqresp.jl | 3 ++- src/types/DelayLtiSystem.jl | 46 +++++++++++++++++++-------------- test/test_delayed_systems.jl | 5 ++-- 6 files changed, 76 insertions(+), 49 deletions(-) diff --git a/example/delayed_lti_timeresp.jl b/example/delayed_lti_timeresp.jl index eea308ccc..9797441c7 100644 --- a/example/delayed_lti_timeresp.jl +++ b/example/delayed_lti_timeresp.jl @@ -1,40 +1,23 @@ -function simulate(sys::DelayLtiSystem, tspan; u=[], x0=Float64[], alg=MethodOfSteps(Tsit5()), kwargs...) - ControlSystems.PartionedStateSpace(sys.P, 1, 1) +using DifferentialEquations - nu = ControlSystems.ninputs(sys) - nx = ControlSystems.nstates(sys) - u = (u == []) ? t -> fill(0.0, nu) : u - x0 = (x0 == []) ? fill(0.0, nx) : x0 - # FIXME: Only works for one single time delay - # FIXME: Should error for non-zero D22 terms - dde = function (dx, x, h, p, t) - d_delayed = P.C2*h(p,t-sys.Tau[1]) + P.D21*u(t-sys.Tau[1])# + P.D22*(t-Tau[1] - dx .= P.A*x + P.B2*d_delayed + P.B1*u(t) - end - h_initial = (p, t) -> zeros(N) - saved_values_y = SavedValues(Float64, Vector{Float64}) - cb = SavingCallback((x,t,integrator)->(P.C1*x + P.D11*u(t)), saved_values_y) +#sys_d = c2d(sys.P) - prob = DDEProblem(dde, x0, h_initial, tspan, constant_lags=sys.Tau) +#function sim_discr(sys::DelayLtiSystem) - sol = solve(prob, alg; saveat=0:0.02:tspan[2], callback=cb, kwargs...) +sys = feedback(2.0, ss(-1.0, 2, 1, 0) * delay(3.0)) - t, x = sol.t, sol.u +@time t, x, saved_y = ControlSystems.simulate2(sys, (0.0,8.0); u=t->[t>0 ? 1.0 : 0.0], saveat=0:0.02:5) - t, x -end - -sys = feedback(1.0, ss(-1, 1, 1, 0) * delay(1.0)) +saved_y +x1 = [x[k][1] for k=1:length(x)] -@time t, x = simulate(sys, (0.0,4.0); u=t->[t>0 ? 1.0 : 0.0], saveat=0:0.02:5) -x1 = [x[k][1] for k=1:length(x)] del = [zeros(50); x1[1:end-50]] # FIXME: Not the real delayed signal... # The following does not work in general... just this specific problem instance diff --git a/src/ControlSystems.jl b/src/ControlSystems.jl index 61553806b..519056e49 100644 --- a/src/ControlSystems.jl +++ b/src/ControlSystems.jl @@ -81,7 +81,8 @@ export LTISystem, # QUESTION: are these used? LaTeXStrings, Requires, IterTools -using Polynomials, OrdinaryDiffEq, Plots, LaTeXStrings, LinearAlgebra +using Polynomials, Plots, LaTeXStrings, LinearAlgebra +using DifferentialEquations, OrdinaryDiffEq, DelayDiffEq export Plots import Base: +, -, *, /, (==), (!=), isapprox, convert, promote_op import Base: getproperty diff --git a/src/delay_systems.jl b/src/delay_systems.jl index ad0e1b21d..07c455faa 100644 --- a/src/delay_systems.jl +++ b/src/delay_systems.jl @@ -2,7 +2,7 @@ function freqresp(sys::DelayLtiSystem, ω::AbstractVector{T}) where {T <: Real} ny = noutputs(sys) nu = ninputs(sys) - P_fr = ControlSystems.freqresp(sys.P, ω); + P_fr = ControlSystems.freqresp(sys.P.P, ω); G_fr = zeros(eltype(P_fr), length(ω), ny, nu) @@ -19,3 +19,38 @@ function freqresp(sys::DelayLtiSystem, ω::AbstractVector{T}) where {T <: Real} return G_fr end + + + +function simulate5(sys::DelayLtiSystem, tspan; u=[], x0=Float64[], alg=MethodOfSteps(Tsit5()), kwargs...) + P = sys.P + + nu = ControlSystems.ninputs(sys) + nx = ControlSystems.nstates(sys) + + u = (u == []) ? t -> fill(0.0, nu) : u + x0 = (x0 == []) ? fill(0.0, nx) : x0 + + # FIXME: Only works for one single time delay + # FIXME: Should error for non-zero D22 terms + dde = function (dx, x, h, p, t) + d_delayed = P.C2*h(p,t-sys.Tau[1]) + P.D21*u(t-sys.Tau[1])# + P.D22*(t-Tau[1] + dx .= P.A*x + P.B2*d_delayed + P.B1*u(t) + end + + h_initial = (p, t) -> zeros(nx) + + + # Saves y (excluding the d(t-τ) contribution) and d + saved_values_y = SavedValues(Float64, Tuple{Vector{Float64}, Vector{Float64}}) + cb = SavingCallback((x,t,integrator) -> (P.C1*x + P.D11*u(t), P.C2*x + P.D21*u(t)), saved_values_y, saveat=0:0.02:tspan[2]) +#P.C1*x + P.D11*u(t) + P.D12*h(t) + prob = DDEProblem(dde, x0, h_initial, tspan, constant_lags=sys.Tau) + + sol = solve(prob, alg, callback=cb; saveat=0:0.02:tspan[2], kwargs...) + + t, x = sol.t, sol.u + + + t, x, saved_values_y +end diff --git a/src/freqresp.jl b/src/freqresp.jl index b883bb4a5..5d2a21e3a 100644 --- a/src/freqresp.jl +++ b/src/freqresp.jl @@ -34,7 +34,8 @@ function _preprocess_for_freqresp(sys::StateSpace) F = hessenberg(A) T = F.Q P = C*T - Q = T\B # TODO Type stability? + Q = T\B # TODO Type stability? # T is unitary, so mutliplication with T' should do the trick + # FIXME; No performance improvement from Hessienberg structure, also weired renaming of matrices StateSpace(F.H, Q, P, D, sys.Ts) end diff --git a/src/types/DelayLtiSystem.jl b/src/types/DelayLtiSystem.jl index b6e6c4910..a39c0bdf2 100644 --- a/src/types/DelayLtiSystem.jl +++ b/src/types/DelayLtiSystem.jl @@ -1,5 +1,5 @@ struct DelayLtiSystem{T} <: LTISystem - P::StateSpace{T,Matrix{T}} + P::PartionedStateSpace{StateSpace{T,Matrix{T}}} Tau::Vector{Float64} # The length of the vector tau implicitly defines the partitionging of P # function DelayLtiSystem(P::StateSpace{T, MT}, Tau::Vector{Float64}) @@ -11,6 +11,21 @@ struct DelayLtiSystem{T} <: LTISystem # end end +# QUESTION: would psys be a good standard variable name for a PartionedStateSpace +# and perhaps dsys for a delayed system, (ambigous with discrete system though) +function DelayLtiSystem{T}(sys::StateSpace, Tau::Vector{Float64}) where T + nu = ninputs(sys) - length(Tau) + ny = noutputs(sys) - length(Tau) + + if nu < 0 || ny < 0 + error("Time vector is too long.") + end + + psys = PartionedStateSpace(sys, ny, nu) + DelayLtiSystem{T}(psys, Tau) +end + +DelayLtiSystem(sys::StateSpace{T,MT}, Tau::Vector{Float64}) where {T, MT} = DelayLtiSystem{T}(sys, Tau) DelayLtiSystem(sys::StateSpace{T,MT}) where {T, MT} = DelayLtiSystem{T}(sys, Float64[]) DelayLtiSystem{T}(sys::StateSpace) where T = DelayLtiSystem{T}(sys, Float64[]) @@ -23,43 +38,34 @@ Base.promote_rule(::Type{T1}, ::Type{DelayLtiSystem{T2}}) where {T1<:Number,T2<: Base.convert(::Type{DelayLtiSystem{T}}, sys::StateSpace) where T = DelayLtiSystem{T}(sys) Base.convert(::Type{DelayLtiSystem{T1}}, d::T2) where {T1,T2 <: Number} = DelayLtiSystem{T1}(ss(d)) -#Base.convert(::Type{DelayLtiSystem{T}}, sys::DelayLtiSystem) where T = DelayLtiSystem{T}(StateSpace{T,Matrix{T}}(sys)) +Base.convert(::Type{DelayLtiSystem{T}}, sys::DelayLtiSystem) where T = DelayLtiSystem{T}(StateSpace{T,Matrix{T}}(sys.P.P), sys.Tau) -ninputs(sys::DelayLtiSystem) = ninputs(sys.P) - length(sys.Tau) -noutputs(sys::DelayLtiSystem) = noutputs(sys.P) - length(sys.Tau) -nstates(sys::DelayLtiSystem) = nstates(sys.P) +ninputs(sys::DelayLtiSystem) = size(sys.P.P, 2) - length(sys.Tau) +noutputs(sys::DelayLtiSystem) = size(sys.P.P, 1) - length(sys.Tau) +nstates(sys::DelayLtiSystem) = nstates(sys.P.P) function +(sys1::DelayLtiSystem, sys2::DelayLtiSystem) - s1 = PartionedStateSpace(sys1.P, ninputs(sys1), noutputs(sys1)) - s2 = PartionedStateSpace(sys2.P, ninputs(sys2), noutputs(sys2)) - - s_new = s1 + s2 + psys_new = sys1.P + sys2.P Tau_new = [sys1.Tau; sys2.Tau] - DelayLtiSystem(s_new.P, Tau_new) + DelayLtiSystem(psys_new.P, Tau_new) end function *(sys1::DelayLtiSystem, sys2::DelayLtiSystem) - s1 = PartionedStateSpace(sys1.P, ninputs(sys1), noutputs(sys1)) - s2 = PartionedStateSpace(sys2.P, ninputs(sys2), noutputs(sys2)) - - s_new = s1 * s2 + psys_new = sys1.P * sys2.P Tau_new = [sys1.Tau; sys2.Tau] - DelayLtiSystem(s_new.P, Tau_new) + DelayLtiSystem(psys_new.P, Tau_new) end function feedback(sys1::DelayLtiSystem, sys2::DelayLtiSystem) - s1 = PartionedStateSpace(sys1.P, ninputs(sys1), noutputs(sys1)) - s2 = PartionedStateSpace(sys2.P, ninputs(sys2), noutputs(sys2)) - - s_new = feedback(s1, s2) + psys_new = feedback(sys1.P, sys2.P) Tau_new = [sys1.Tau; sys2.Tau] - DelayLtiSystem(s_new.P, Tau_new) + DelayLtiSystem(psys_new.P, Tau_new) end function delay(tau::Real, T::Type{<:Number}=Float64) diff --git a/test/test_delayed_systems.jl b/test/test_delayed_systems.jl index a48b6fc96..743d9039a 100644 --- a/test/test_delayed_systems.jl +++ b/test/test_delayed_systems.jl @@ -1,7 +1,8 @@ ω = 0.0:8 -typeof(promote(delay(0.2), ss(1))[1]) == DelayLtiSystem{Float64} -typeof(promote(delay(0.2), ss(1.0 + im))[1]) == DelayLtiSystem{Float64} +# broken: typeof(promote(delay(0.2), ss(1))[1]) == DelayLtiSystem{Float64} + +typeof(promote(delay(0.2), ss(1.0 + im))[1]) == DelayLtiSystem{Complex{Float64}} freqresp(delay(1), ω)[:] ≈ exp.(-im*ω) freqresp(delay(2.5), ω)[:] ≈ exp.(-2.5*im*ω) From 26e7e9852f9879ba091b624f8ffb61953cba6f82 Mon Sep 17 00:00:00 2001 From: olof3 Date: Wed, 23 Jan 2019 18:48:53 +0100 Subject: [PATCH 15/30] Updates to delayed LTI. --- example/delayed_lti_timeresp.jl | 28 ++++++++++++++-------------- src/delay_systems.jl | 14 ++++++++++---- 2 files changed, 24 insertions(+), 18 deletions(-) diff --git a/example/delayed_lti_timeresp.jl b/example/delayed_lti_timeresp.jl index 9797441c7..58817b128 100644 --- a/example/delayed_lti_timeresp.jl +++ b/example/delayed_lti_timeresp.jl @@ -1,24 +1,24 @@ using DifferentialEquations - - - - +plotly() #sys_d = c2d(sys.P) -#function sim_discr(sys::DelayLtiSystem) - -sys = feedback(2.0, ss(-1.0, 2, 1, 0) * delay(3.0)) - -@time t, x, saved_y = ControlSystems.simulate2(sys, (0.0,8.0); u=t->[t>0 ? 1.0 : 0.0], saveat=0:0.02:5) +sys = feedback(1.0, ss(-1.0, 2, 1, 0) * (delay(2.0) + delay(3.0))) +@time t, x, saved_y = ControlSystems.simulate5(sys, (0.0,8.0); u=t->[t>0 ? 1.0 : 0.0], saveat=0:0.02:8) -saved_y -x1 = [x[k][1] for k=1:length(x)] +y = hcat([saved_y.saveval[k][1] for k=1:length(t)]...) +d = hcat([saved_y.saveval[k][2] for k=1:length(t)]...) +for k=1:length(sys.Tau) + N_del = Integer(50*sys.Tau[k]) + dk = [zeros(N_del); d[k, 1:end-N_del]] -del = [zeros(50); x1[1:end-50]] # FIXME: Not the real delayed signal... + for j=1:length(t) + y[:, j] .+= sys.P.D12[:, k] * dk[j] + end +end -# The following does not work in general... just this specific problem instance -plot(t, ones(size(t)) .- x1) +plot(t, y') +gui() diff --git a/src/delay_systems.jl b/src/delay_systems.jl index 07c455faa..4b2ddbdce 100644 --- a/src/delay_systems.jl +++ b/src/delay_systems.jl @@ -25,17 +25,23 @@ end function simulate5(sys::DelayLtiSystem, tspan; u=[], x0=Float64[], alg=MethodOfSteps(Tsit5()), kwargs...) P = sys.P + if ~iszero(P.D22) + error("non-zero D22-matrix block is not supported") # Due to limitations in differential equations + end + nu = ControlSystems.ninputs(sys) nx = ControlSystems.nstates(sys) u = (u == []) ? t -> fill(0.0, nu) : u x0 = (x0 == []) ? fill(0.0, nx) : x0 - # FIXME: Only works for one single time delay - # FIXME: Should error for non-zero D22 terms dde = function (dx, x, h, p, t) - d_delayed = P.C2*h(p,t-sys.Tau[1]) + P.D21*u(t-sys.Tau[1])# + P.D22*(t-Tau[1] - dx .= P.A*x + P.B2*d_delayed + P.B1*u(t) + dx .= P.A*x + P.B1*u(t) + for k=1:length(sys.Tau) # Add each of the delayed signals + dk_delayed = dot(P.C2[k,:], h(p,t-sys.Tau[k])) + dot(P.D21[k,:], u(t-sys.Tau[k]))# + P.D22*(t-Tau[1] + dx .+= P.B2[:, k] * dk_delayed + end + #d_delayed = zeros(size(P.C2,1)) end h_initial = (p, t) -> zeros(nx) From 39da60e85975bfdd1bc83d45ac15d0dc05be7179 Mon Sep 17 00:00:00 2001 From: olof3 Date: Thu, 24 Jan 2019 12:20:42 +0100 Subject: [PATCH 16/30] Updates delayed LTI systems. --- REQUIRE | 10 +++++ example/delayed_lti_timeresp.jl | 4 -- src/delay_systems.jl | 5 ++- src/types/DelayLtiSystem.jl | 7 +++ src/types/PartionedStateSpace.jl | 8 ++-- test/test_delayed_systems.jl | 77 +++++++++++++++++++++++++------- 6 files changed, 84 insertions(+), 27 deletions(-) create mode 100644 REQUIRE diff --git a/REQUIRE b/REQUIRE new file mode 100644 index 000000000..1deda6f8d --- /dev/null +++ b/REQUIRE @@ -0,0 +1,10 @@ +julia 0.7 +Plots +Polynomials +LaTeXStrings +DifferentialEquations +DelayDiffEq +OrdinaryDiffEq +IterTools +Colors +DSP diff --git a/example/delayed_lti_timeresp.jl b/example/delayed_lti_timeresp.jl index 58817b128..aedf74a34 100644 --- a/example/delayed_lti_timeresp.jl +++ b/example/delayed_lti_timeresp.jl @@ -1,9 +1,5 @@ -using DifferentialEquations - plotly() -#sys_d = c2d(sys.P) - sys = feedback(1.0, ss(-1.0, 2, 1, 0) * (delay(2.0) + delay(3.0))) @time t, x, saved_y = ControlSystems.simulate5(sys, (0.0,8.0); u=t->[t>0 ? 1.0 : 0.0], saveat=0:0.02:8) diff --git a/src/delay_systems.jl b/src/delay_systems.jl index 4b2ddbdce..8651ceeb8 100644 --- a/src/delay_systems.jl +++ b/src/delay_systems.jl @@ -12,9 +12,10 @@ function freqresp(sys::DelayLtiSystem, ω::AbstractVector{T}) where {T <: Real} P21_fr = P_fr[ω_idx, ny+1:end, 1:nu] P22_fr = P_fr[ω_idx, ny+1:end, nu+1:end] - delay_matrix_fr = Diagonal(exp.(im*sys.Tau*ω[ω_idx])) # Frequency response of the diagonal matrix with delays + delay_matrix_inv_fr = Diagonal(exp.(im*sys.Tau*ω[ω_idx])) # 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_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 diff --git a/src/types/DelayLtiSystem.jl b/src/types/DelayLtiSystem.jl index a39c0bdf2..24e7c9713 100644 --- a/src/types/DelayLtiSystem.jl +++ b/src/types/DelayLtiSystem.jl @@ -41,6 +41,13 @@ Base.convert(::Type{DelayLtiSystem{T1}}, d::T2) where {T1,T2 <: Number} = DelayL Base.convert(::Type{DelayLtiSystem{T}}, sys::DelayLtiSystem) where T = DelayLtiSystem{T}(StateSpace{T,Matrix{T}}(sys.P.P), sys.Tau) +function *(sys::DelayLtiSystem, n::Number) + new_C = [sys.P.C1*n; sys.P.C2] + new_D = [sys.P.D11*n sys.P.D12; sys.P.D21*n sys.P.D22] + return DelayLtiSystem(StateSpace(sys.P.A, sys.P.B, new_C, new_D, sys.P.Ts), sys.Tau) +end +*(n::Number, sys::DelayLtiSystem) = *(sys, n) + ninputs(sys::DelayLtiSystem) = size(sys.P.P, 2) - length(sys.Tau) noutputs(sys::DelayLtiSystem) = size(sys.P.P, 1) - length(sys.Tau) nstates(sys::DelayLtiSystem) = nstates(sys.P.P) diff --git a/src/types/PartionedStateSpace.jl b/src/types/PartionedStateSpace.jl index 1901f4db6..cffe82df3 100644 --- a/src/types/PartionedStateSpace.jl +++ b/src/types/PartionedStateSpace.jl @@ -114,14 +114,14 @@ end # QUESTION: Should perhaps algebraic loops?! Perhaps issue warning if P1(∞)*P2(∞) > 1 function feedback(s1::PartionedStateSpace, s2::PartionedStateSpace) - X_11 = [-s2.D11*s1.C1 -s2.C1] - X_21 = [s1.C1 -s1.D11*s2.C1] + X_11 = (I + s2.D11*s1.D11)\[-s2.D11*s1.C1 -s2.C1] + X_21 = (I + s1.D11*s2.D11)\[s1.C1 -s1.D11*s2.C1] # For the case of two outputs # X_12 = [I -s2.D11 -s2.D11*s1.D12 -s2.D12] # X_22 = [s1.D11 I -s1.D12 s1.D11*s2.D12] - X_12 = [I -s2.D11*s1.D12 -s2.D12] - X_22 = [s1.D11 -s1.D12 s1.D11*s2.D12] + X_12 = (I + s2.D11*s1.D11)\[I -s2.D11*s1.D12 -s2.D12] + X_22 = (I + s1.D11*s2.D11)\[s1.D11 s1.D12 -s1.D11*s2.D12] diff --git a/test/test_delayed_systems.jl b/test/test_delayed_systems.jl index 743d9039a..710e6c589 100644 --- a/test/test_delayed_systems.jl +++ b/test/test_delayed_systems.jl @@ -2,41 +2,84 @@ # broken: typeof(promote(delay(0.2), ss(1))[1]) == DelayLtiSystem{Float64} -typeof(promote(delay(0.2), ss(1.0 + im))[1]) == DelayLtiSystem{Complex{Float64}} +@test typeof(promote(delay(0.2), ss(1.0 + im))[1]) == DelayLtiSystem{Complex{Float64}} -freqresp(delay(1), ω)[:] ≈ exp.(-im*ω) -freqresp(delay(2.5), ω)[:] ≈ exp.(-2.5*im*ω) +@test freqresp(delay(1), ω)[:] ≈ exp.(-im*ω) rtol=1e-15 +@test freqresp(delay(2.5), ω)[:] ≈ exp.(-2.5im*ω) rtol=1e-15 +@test freqresp(3.5*delay(2.5), ω)[:] ≈ 3.5*exp.(-2.5im*ω) rtol=1e-15 +@test freqresp(delay(2.5)*1.5, ω)[:] ≈ exp.(-2.5im*ω)*1.5 rtol=1e-15 +# Stritcly proper system P1 = DelayLtiSystem(ss(-1.0, 1, 1, 0)) P1_fr = 1 ./ (im*ω .+ 1) +@test freqresp(P1, ω)[:] == P1_fr +# Not stritcly proper system P2 = DelayLtiSystem(ss(-2.0, -1, 1, 1)) # (s+1)/(s+2) P2_fr = (im*ω .+ 1) ./ (im*ω .+ 2) - +@test freqresp(P2, ω)[:] ≈ P2_fr rtol=1e-15 # Addition -freqresp(P1 + delay(1), ω)[:] ≈ P1_fr .+ exp.(-im*ω) +@test_broken freqresp(1 + delay(1), ω)[:] ≈ 1 .+ exp.(-im*ω) # FIXME; Add suitable conversion from Int64 to DelayLtiSystem +@test freqresp(P1 + delay(1), ω)[:] ≈ P1_fr .+ exp.(-im*ω) + #FIXME: the following gives a crash.. freqresp(P1 - delay(1), ω)[:] ≈ P1_fr .+ exp.(-im*ω) # Multiplication -freqresp(P1 * delay(1), ω)[:] ≈ P1_fr .* exp.(-im*ω) -freqresp(delay(1) * P1, ω)[:] ≈ P1_fr .* exp.(-im*ω) +@test freqresp(P1 * delay(1), ω)[:] ≈ P1_fr .* exp.(-im*ω) rtol=1e-15 +@test freqresp(delay(1) * P1, ω)[:] ≈ P1_fr .* exp.(-im*ω) rtol=1e-15 -freqresp(P2 * delay(1), ω)[:] ≈ P2_fr .* exp.(-im*ω) -freqresp(delay(1) * P2, ω)[:] ≈ P2_fr .* exp.(-im*ω) +@test freqresp(P2 * delay(1), ω)[:] ≈ P2_fr .* exp.(-im*ω) rtol=1e-15 +@test freqresp(delay(1) * P2, ω)[:] ≈ P2_fr .* exp.(-im*ω) rtol=1e-15 # Feedback # The first tests don't include delays, but the linear system is of DelayLtiForm type # (very simple system so easy to troubleshoot) -freqresp(feedback(1.0, P1), ω)[:] ≈ 1 ./ (1 .+ P1_fr) -freqresp(feedback(P1, 1.0), ω)[:] ≈ P1_fr ./ (1 .+ P1_fr) -freqresp(feedback(P1, P1), ω)[:] ≈ P1_fr ./ (1 .+ P1_fr .* P1_fr) - -freqresp(feedback(P1, delay(1)), ω)[:] ≈ P1_fr ./ (1 .+ exp.(-im*ω) .* P1_fr) -freqresp(feedback(delay(1), P1), ω)[:] ≈ exp.(-im*ω) ./ (1 .+ exp.(-im*ω) .* P1_fr) #FIXME: Answer is Inf, but should give error.. -freqresp(feedback(P1*delay(1), 1.0), ω)[:] ≈ P1_fr .* exp.(-im*ω) ./ (1 .+ exp.(-im*ω) .* P1_fr) -freqresp(feedback(1.0, P1*delay(1)), ω)[:] ≈ 1 ./ (1 .+ exp.(-im*ω) .* P1_fr) +@test freqresp(feedback(1.0, P1), ω)[:] ≈ 1 ./ (1 .+ P1_fr) rtol=1e-15 +@test freqresp(feedback(P1, 1.0), ω)[:] ≈ P1_fr ./ (1 .+ P1_fr) rtol=1e-15 +@test freqresp(feedback(P1, P1), ω)[:] ≈ P1_fr ./ (1 .+ P1_fr .* P1_fr) rtol=1e-15 + +@test freqresp(feedback(1.0, DelayLtiSystem(ss(0.5))), [0])[:] == [2/3] +@test freqresp(feedback(1.0, P2), ω)[:] ≈ 1 ./ (1 .+ P2_fr) + + +freqresp(feedback(1.0, ss(0.5) + 0.5*delay(2) + 0.5*delay(3)), ω)[:] +freqresp(feedback(1.0, G), ω)[:] + + +@test freqresp(feedback(0.5, delay(2.0)), ω) ≈ 0.5 ./ (1 .+ 0.5*exp.(-2im*ω)) +@test freqresp(feedback(delay(2.0), 0.5), ω) ≈ exp.(-2im*ω) ./ (1 .+ 0.5*exp.(-2im*ω)) + +@test freqresp(feedback(P1, delay(1)), ω)[:] ≈ P1_fr ./ (1 .+ exp.(-im*ω) .* P1_fr) rtol=1e-15 +@test freqresp(feedback(delay(1), P1), ω)[:] ≈ exp.(-im*ω) ./ (1 .+ exp.(-im*ω) .* P1_fr) #FIXME: Answer is Inf, but should give error.. rtol=1e-15 +@test freqresp(feedback(P1*delay(1), 1.0), ω)[:] ≈ P1_fr .* exp.(-im*ω) ./ (1 .+ exp.(-im*ω) .* P1_fr) rtol=1e-15 +@test freqresp(feedback(1.0, P1*delay(1)), ω)[:] ≈ 1 ./ (1 .+ exp.(-im*ω) .* P1_fr) rtol=1e-15 + +@test freqresp(feedback(1.0, P2*0.5*(ss(1.0) + delay(2))), ω)[:] ≈ 1 ./(1 .+ P2_fr .* 0.5.*(1 .+ exp.(-2*im*ω))) + + +@test_broken freqresp(1.0 + delay(2), ω)[:] # TODO: Add addition for DelayLtiSystem. how to do this in a convenient ammner? + +G = ss(0.5) + 0.5*delay(2)# + 0.5*delay(3) +G_fr = 0.5 .+ 0.5*exp.(-2*im*ω)# + 0.5*exp.(-3*im*ω) +@test freqresp(G, ω)[:] ≈ G_fr rtol=1e-15 + +@test freqresp(feedback(1.0, P1*G), ω)[:] ≈ 1 ./(1 .+ P1_fr .* G_fr) rtol=1e-15 +@test freqresp(feedback(P1, G), ω)[:] ≈ P1_fr ./(1 .+ P1_fr .* G_fr) rtol=1e-15 + +@test freqresp(feedback(P2, G), ω)[:] ≈ P2_fr ./(1 .+ P2_fr .* G_fr) rtol=1e-15 + +sys = feedback(1.0, P1 * (delay(2) + delay(3))) +expected_sys_fr = 1.0 ./ (P1_fr .* (exp.(-2*im*ω) + exp.(-3*im*ω)) .+ 1) +@test freqresp(sys, ω)[:] ≈ expected_sys_fr rtol=1e-14 + +## Multiple Delays +G = ss(1.0) + delay(2) + delay(3) +G_fr = 1 .+ exp.(-2*im*ω) .+ exp.(-3*im*ω) +@test freqresp(G, ω)[:] ≈ G_fr rtol=1e-15 +@test freqresp(feedback(1.0, G), ω)[:] ≈ 1 ./(1 .+ G_fr) # Somewhat pathological system though + #FIXME: A lot more tests, including MIMO systems in particular From 9307374a7db092947c8a6bdc0c5428eb5c38edf1 Mon Sep 17 00:00:00 2001 From: olof3 Date: Thu, 24 Jan 2019 13:50:52 +0100 Subject: [PATCH 17/30] Updates delayed LTI systems. --- example/delayed_lti_timeresp.jl | 18 +++--------- src/delay_systems.jl | 44 ++++++++++++++++++----------- src/types/PartionedStateSpace.jl | 4 +-- test/runtests.jl | 4 ++- test/test_delayed_systems.jl | 23 +++++++-------- test/test_partitioned_statespace.jl | 31 ++++++++++---------- 6 files changed, 62 insertions(+), 62 deletions(-) diff --git a/example/delayed_lti_timeresp.jl b/example/delayed_lti_timeresp.jl index aedf74a34..a7e44e591 100644 --- a/example/delayed_lti_timeresp.jl +++ b/example/delayed_lti_timeresp.jl @@ -1,20 +1,10 @@ +using Plots plotly() -sys = feedback(1.0, ss(-1.0, 2, 1, 0) * (delay(2.0) + delay(3.0))) +sys = feedback(1.0, ss(-1.0, 2, 1, 0) * (delay(2.0) + delay(3.0) + delay(2.5))) -@time t, x, saved_y = ControlSystems.simulate5(sys, (0.0,8.0); u=t->[t>0 ? 1.0 : 0.0], saveat=0:0.02:8) - -y = hcat([saved_y.saveval[k][1] for k=1:length(t)]...) -d = hcat([saved_y.saveval[k][2] for k=1:length(t)]...) - -for k=1:length(sys.Tau) - N_del = Integer(50*sys.Tau[k]) - dk = [zeros(N_del); d[k, 1:end-N_del]] - - for j=1:length(t) - y[:, j] .+= sys.P.D12[:, k] * dk[j] - end -end +t = 0:0.02:8 +@time t, x, y = lsim(sys, t; u=t->[t>=0 ? 1.0 : 0.0]) plot(t, y') gui() diff --git a/src/delay_systems.jl b/src/delay_systems.jl index 8651ceeb8..6a34d3189 100644 --- a/src/delay_systems.jl +++ b/src/delay_systems.jl @@ -23,41 +23,51 @@ end -function simulate5(sys::DelayLtiSystem, tspan; u=[], x0=Float64[], alg=MethodOfSteps(Tsit5()), kwargs...) +function lsim(sys::DelayLtiSystem, t::AbstractArray{<:Real}; u=(t -> fill(0.0, ninputs(sys))), x0=fill(0.0, nstates(sys)), alg=MethodOfSteps(Tsit5()), kwargs...) P = sys.P if ~iszero(P.D22) error("non-zero D22-matrix block is not supported") # Due to limitations in differential equations end - nu = ControlSystems.ninputs(sys) - nx = ControlSystems.nstates(sys) - - u = (u == []) ? t -> fill(0.0, nu) : u - x0 = (x0 == []) ? fill(0.0, nx) : x0 + dt = t[2] - t[1] + if ~all(diff(t) .≈ dt) # QUESTION Does this work or are there precision problems? + error("The t-vector should be uniformly spaced, t[2] - t[1] = $dt.") # Perhaps dedicated function for checking this? + end + # Slightly more complicated definition since the d signal is not directly available dde = function (dx, x, h, p, t) dx .= P.A*x + P.B1*u(t) for k=1:length(sys.Tau) # Add each of the delayed signals - dk_delayed = dot(P.C2[k,:], h(p,t-sys.Tau[k])) + dot(P.D21[k,:], u(t-sys.Tau[k]))# + P.D22*(t-Tau[1] + dk_delayed = dot(P.C2[k,:], h(p,t-sys.Tau[k])) + dot(P.D21[k,:], u(t-sys.Tau[k])) dx .+= P.B2[:, k] * dk_delayed end - #d_delayed = zeros(size(P.C2,1)) end - h_initial = (p, t) -> zeros(nx) - + h_initial = (p, t) -> zeros(nstates(sys)) # Saves y (excluding the d(t-τ) contribution) and d - saved_values_y = SavedValues(Float64, Tuple{Vector{Float64}, Vector{Float64}}) - cb = SavingCallback((x,t,integrator) -> (P.C1*x + P.D11*u(t), P.C2*x + P.D21*u(t)), saved_values_y, saveat=0:0.02:tspan[2]) -#P.C1*x + P.D11*u(t) + P.D12*h(t) - prob = DDEProblem(dde, x0, h_initial, tspan, constant_lags=sys.Tau) + saved_values = SavedValues(Float64, Tuple{Vector{Float64}, Vector{Float64}}) + cb = SavingCallback((x,t,integrator) -> (P.C1*x + P.D11*u(t), P.C2*x + P.D21*u(t)), saved_values, saveat=t) + + prob = DDEProblem(dde, x0, h_initial, (0.0, 8.0), constant_lags=sys.Tau) - sol = solve(prob, alg, callback=cb; saveat=0:0.02:tspan[2], kwargs...) + sol = solve(prob, alg, callback=cb; saveat=t, kwargs...) - t, x = sol.t, sol.u + x = sol.u # the states are labeled u in DifferentialEquations + y = hcat([saved_values.saveval[k][1] for k=1:length(t)]...) + d = hcat([saved_values.saveval[k][2] for k=1:length(t)]...) + + # Account for the effect of the delayed d-signal on y + for k=1:length(sys.Tau) + N_del = Integer(sys.Tau[k] / dt) + dk = [zeros(N_del); d[k, 1:end-N_del]] + + for j=1:length(t) + y[:, j] .+= sys.P.D12[:, k] * dk[j] + end + end - t, x, saved_values_y + t, x, y end diff --git a/src/types/PartionedStateSpace.jl b/src/types/PartionedStateSpace.jl index cffe82df3..84586569c 100644 --- a/src/types/PartionedStateSpace.jl +++ b/src/types/PartionedStateSpace.jl @@ -119,12 +119,10 @@ function feedback(s1::PartionedStateSpace, s2::PartionedStateSpace) # For the case of two outputs # X_12 = [I -s2.D11 -s2.D11*s1.D12 -s2.D12] - # X_22 = [s1.D11 I -s1.D12 s1.D11*s2.D12] + # X_22 = [s1.D11 I s1.D12 -s1.D11*s2.D12] X_12 = (I + s2.D11*s1.D11)\[I -s2.D11*s1.D12 -s2.D12] X_22 = (I + s1.D11*s2.D11)\[s1.D11 s1.D12 -s1.D11*s2.D12] - - A = [s1.B1 * X_11 ; s2.B1 * X_21] + blkdiag(s1.A, s2.A) B = [s1.B1 * X_12 ; s2.B1 * X_22] diff --git a/test/runtests.jl b/test/runtests.jl index dbf4b8a26..e1f449fb6 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -23,7 +23,9 @@ my_tests = ["test_statespace", "test_analysis", "test_matrix_comps", "test_lqg", - "test_synthesis"] + "test_synthesis", + "test_partitioned_statespace", + "test_delayed_systems"] # try diff --git a/test/test_delayed_systems.jl b/test/test_delayed_systems.jl index 710e6c589..54422e0d2 100644 --- a/test/test_delayed_systems.jl +++ b/test/test_delayed_systems.jl @@ -4,6 +4,7 @@ @test typeof(promote(delay(0.2), ss(1.0 + im))[1]) == DelayLtiSystem{Complex{Float64}} +# Extremely baseic tests @test freqresp(delay(1), ω)[:] ≈ exp.(-im*ω) rtol=1e-15 @test freqresp(delay(2.5), ω)[:] ≈ exp.(-2.5im*ω) rtol=1e-15 @test freqresp(3.5*delay(2.5), ω)[:] ≈ 3.5*exp.(-2.5im*ω) rtol=1e-15 @@ -19,14 +20,15 @@ P2 = DelayLtiSystem(ss(-2.0, -1, 1, 1)) # (s+1)/(s+2) P2_fr = (im*ω .+ 1) ./ (im*ω .+ 2) @test freqresp(P2, ω)[:] ≈ P2_fr rtol=1e-15 -# Addition + +## Addition @test_broken freqresp(1 + delay(1), ω)[:] ≈ 1 .+ exp.(-im*ω) # FIXME; Add suitable conversion from Int64 to DelayLtiSystem @test freqresp(P1 + delay(1), ω)[:] ≈ P1_fr .+ exp.(-im*ω) #FIXME: the following gives a crash.. freqresp(P1 - delay(1), ω)[:] ≈ P1_fr .+ exp.(-im*ω) -# Multiplication +## Multiplication @test freqresp(P1 * delay(1), ω)[:] ≈ P1_fr .* exp.(-im*ω) rtol=1e-15 @test freqresp(delay(1) * P1, ω)[:] ≈ P1_fr .* exp.(-im*ω) rtol=1e-15 @@ -34,7 +36,7 @@ P2_fr = (im*ω .+ 1) ./ (im*ω .+ 2) @test freqresp(delay(1) * P2, ω)[:] ≈ P2_fr .* exp.(-im*ω) rtol=1e-15 -# Feedback +## Feedback # The first tests don't include delays, but the linear system is of DelayLtiForm type # (very simple system so easy to troubleshoot) @test freqresp(feedback(1.0, P1), ω)[:] ≈ 1 ./ (1 .+ P1_fr) rtol=1e-15 @@ -44,11 +46,6 @@ P2_fr = (im*ω .+ 1) ./ (im*ω .+ 2) @test freqresp(feedback(1.0, DelayLtiSystem(ss(0.5))), [0])[:] == [2/3] @test freqresp(feedback(1.0, P2), ω)[:] ≈ 1 ./ (1 .+ P2_fr) - -freqresp(feedback(1.0, ss(0.5) + 0.5*delay(2) + 0.5*delay(3)), ω)[:] -freqresp(feedback(1.0, G), ω)[:] - - @test freqresp(feedback(0.5, delay(2.0)), ω) ≈ 0.5 ./ (1 .+ 0.5*exp.(-2im*ω)) @test freqresp(feedback(delay(2.0), 0.5), ω) ≈ exp.(-2im*ω) ./ (1 .+ 0.5*exp.(-2im*ω)) @@ -71,9 +68,6 @@ G_fr = 0.5 .+ 0.5*exp.(-2*im*ω)# + 0.5*exp.(-3*im*ω) @test freqresp(feedback(P2, G), ω)[:] ≈ P2_fr ./(1 .+ P2_fr .* G_fr) rtol=1e-15 -sys = feedback(1.0, P1 * (delay(2) + delay(3))) -expected_sys_fr = 1.0 ./ (P1_fr .* (exp.(-2*im*ω) + exp.(-3*im*ω)) .+ 1) -@test freqresp(sys, ω)[:] ≈ expected_sys_fr rtol=1e-14 ## Multiple Delays G = ss(1.0) + delay(2) + delay(3) @@ -81,5 +75,12 @@ G_fr = 1 .+ exp.(-2*im*ω) .+ exp.(-3*im*ω) @test freqresp(G, ω)[:] ≈ G_fr rtol=1e-15 @test freqresp(feedback(1.0, G), ω)[:] ≈ 1 ./(1 .+ G_fr) # Somewhat pathological system though +sys = feedback(1.0, P1 * (delay(2) + delay(3))) +expected_sys_fr = 1.0 ./ (P1_fr .* (exp.(-2*im*ω) + exp.(-3*im*ω)) .+ 1) + +@test freqresp(sys, ω)[:] ≈ expected_sys_fr rtol=1e-14 + +@test freqresp(feedback(1.0*P1, G*G*P2*P2), ω)[:] ≈ P1_fr ./(1 .+ (G_fr.^2).*P1_fr.*P2_fr.^2) rtol=1e-15# Somewhat pathological system though + #FIXME: A lot more tests, including MIMO systems in particular diff --git a/test/test_partitioned_statespace.jl b/test/test_partitioned_statespace.jl index cee4a5034..03dd68a1c 100644 --- a/test/test_partitioned_statespace.jl +++ b/test/test_partitioned_statespace.jl @@ -23,19 +23,18 @@ sys1.D22 == matrix(9.0) ## -sys1 = ControlSystems.PartionedStateSpace(ss(fill(1.0, 2, 2), fill(2.0, 2, 5), fill(3.0, 7, 2), fill(4.0, 7, 5)), 2, 3) - -sys1.A == fill(1.0, 2, 2) -sys1.B1 == fill(2.0, 2, 2) -sys1.B2 == fill(2.0, 2, 3) -sys1.C1 == fill(3.0, 3, 2) -sys1.C2 == fill(3.0, 4, 2) -sys1.D11 == fill(4.0, 3, 2) -sys1.D12 == fill(4.0, 3, 3) -sys1.D21 == fill(4.0, 4, 2) -sys1.D22 == fill(4.0, 4, 3) - - -# Simple test -(sys1 + sys1).P[1, 1] == (sys1.P[1,1] + sys1.P[1,1]) -(sys1 * sys1).P[1, 1] == (sys1.P[1,1] * sys1.P[1,1]) +sys2 = ControlSystems.PartionedStateSpace(ss(fill(1.0, 2, 2), fill(2.0, 2, 5), fill(3.0, 7, 2), fill(4.0, 7, 5)), 2, 3) + +@test sys2.A == fill(1.0, 2, 2) +@test sys2.B1 == fill(2.0, 2, 2) +@test sys2.B2 == fill(2.0, 2, 3) +@test sys2.C1 == fill(3.0, 3, 2) +@test sys2.C2 == fill(3.0, 4, 2) +@test sys2.D11 == fill(4.0, 3, 2) +@test sys2.D12 == fill(4.0, 3, 3) +@test sys2.D21 == fill(4.0, 4, 2) +@test sys2.D22 == fill(4.0, 4, 3) + + +# TODO: Add some tests for interconnections, implicitly tested through delay system implementations though +@test (sys1 + sys1).P[1, 1] == (sys1.P[1,1] + sys1.P[1,1]) From 66d01a65a6a2dc1029d8ecfd7170be1fa8ca2114 Mon Sep 17 00:00:00 2001 From: olof3 Date: Thu, 24 Jan 2019 17:12:36 +0100 Subject: [PATCH 18/30] Minor typo. --- src/types/PartionedStateSpace.jl | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/types/PartionedStateSpace.jl b/src/types/PartionedStateSpace.jl index 84586569c..d3c954ae6 100644 --- a/src/types/PartionedStateSpace.jl +++ b/src/types/PartionedStateSpace.jl @@ -111,8 +111,7 @@ end -# QUESTION: Should perhaps algebraic loops?! Perhaps issue warning if P1(∞)*P2(∞) > 1 - +# QUESTION: What about algebraic loops and well-posedness?! Perhaps issue warning if P1(∞)*P2(∞) > 1 function feedback(s1::PartionedStateSpace, s2::PartionedStateSpace) X_11 = (I + s2.D11*s1.D11)\[-s2.D11*s1.C1 -s2.C1] X_21 = (I + s1.D11*s2.D11)\[s1.C1 -s1.D11*s2.C1] From b373567a6f3a8b6cbc24bcaa1669f91e38043f97 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mattias=20F=C3=A4lt?= Date: Fri, 1 Feb 2019 21:07:11 +0100 Subject: [PATCH 19/30] WIP delayed lsim --- REQUIRE | 2 +- example/delayed_lti_timeresp.jl | 18 ++++- src/ControlSystems.jl | 3 +- src/delay_systems.jl | 123 ++++++++++++++++++++++++------- src/types/PartionedStateSpace.jl | 2 +- 5 files changed, 119 insertions(+), 29 deletions(-) diff --git a/REQUIRE b/REQUIRE index 1deda6f8d..7c3ca060f 100644 --- a/REQUIRE +++ b/REQUIRE @@ -2,9 +2,9 @@ julia 0.7 Plots Polynomials LaTeXStrings -DifferentialEquations DelayDiffEq OrdinaryDiffEq IterTools Colors DSP +Interpolations diff --git a/example/delayed_lti_timeresp.jl b/example/delayed_lti_timeresp.jl index a7e44e591..4b4a46681 100644 --- a/example/delayed_lti_timeresp.jl +++ b/example/delayed_lti_timeresp.jl @@ -4,7 +4,23 @@ plotly() sys = feedback(1.0, ss(-1.0, 2, 1, 0) * (delay(2.0) + delay(3.0) + delay(2.5))) t = 0:0.02:8 -@time t, x, y = lsim(sys, t; u=t->[t>=0 ? 1.0 : 0.0]) +@time lsim(sys, t-> [t>=0 ? 1.0 : 0.0], t) +@time t, x, y = lsim(sys, [1.0], t) +@time t, x, y = lsim(sys, (out, t) -> (out .= (t>=0 ? 1.0 : 0.0)), t) +@time t, x, y = lsim(sys, (out, t) -> (out[1] = (t>=0 ? 1.0 : 0.0)), t) + +function u0(out,t) + if t > 0 + out[1] = 1 + else + out[1] = 0 + end + return +end + +@time t, x, y = lsim(sys, t; u=u0) plot(t, y') gui() + +@time ControlSystems._lsim(sys, t, (out,t)-> out[1] = (t>=0 ? 1.0 : 0.0)) diff --git a/src/ControlSystems.jl b/src/ControlSystems.jl index 519056e49..0c61715f0 100644 --- a/src/ControlSystems.jl +++ b/src/ControlSystems.jl @@ -82,11 +82,12 @@ export LTISystem, # QUESTION: are these used? LaTeXStrings, Requires, IterTools using Polynomials, Plots, LaTeXStrings, LinearAlgebra -using DifferentialEquations, OrdinaryDiffEq, DelayDiffEq +using OrdinaryDiffEq, DelayDiffEq export Plots import Base: +, -, *, /, (==), (!=), isapprox, convert, promote_op import Base: getproperty import LinearAlgebra: BlasFloat +import Interpolations export lyap # Make sure LinearAlgebra.lyap is available import Printf, Colors import DSP: conv diff --git a/src/delay_systems.jl b/src/delay_systems.jl index 6a34d3189..a530e8fde 100644 --- a/src/delay_systems.jl +++ b/src/delay_systems.jl @@ -21,9 +21,67 @@ function freqresp(sys::DelayLtiSystem, ω::AbstractVector{T}) where {T <: Real} return G_fr end +struct FunctionWrapper <: Function + f::Function +end +(fv::FunctionWrapper)(dx, x, h!, p, t) = fv.f(dx, x, h!, p, t) + +struct UWrapper <: Function + f::Function +end +(fv::UWrapper)(out, t) = fv.f(out, t) + + +""" + `t, x, y = lsim(sys::DelayLtiSystem, t::AbstractArray{<:Real}; u=(out, t) -> (out .= 0), x0=fill(0.0, nstates(sys)), alg=MethodOfSteps(Tsit5()), kwargs...)` + + Simulate system `sys`, over time `t`, using input signal `u`, with initial state `x0`, using method `alg` . + + Arguments: + + `t`: Has to be an `AbstractVector` with equidistant time samples (`t[i] - t[i-1]` constant) + `u`: Function to determine control signal `ut` at a time `t`, on any of the following forms: + Can be a constant `Number` or `Vector`, interpreted as `ut .= u` , or + Function `ut .= u(t)`, or + In-place function `u(ut, t)`. (Slightly more effienct) + + Returns: `x` and `y` at times `t`. +""" +function lsim(sys::DelayLtiSystem{T}, u, t::AbstractArray{<:T}; x0=fill(zero(T), nstates(sys)), alg=MethodOfSteps(Tsit5())) where T + # Make u! in-place function of u + u! = if isa(u, Number) || isa(u,AbstractVector) # Allow for u to be a constant number or vector + println("Number vector") + (uout, t) -> uout .= u + elseif DiffEqBase.isinplace(u, 2) # If u is an inplace (more than 1 argument function) + println("Inplace") + u + else # If u is a regular u(t) function + println("Outplace") + (out, t) -> (out .= u(t)) + end + _lsim(sys, UWrapper(u!), t, x0, alg) +end +function dde_param(dx, x, h!, p, t) + A, B1, B2, C2, D21, Tau, u!, uout, hout, tmp = p[1],p[2],p[3],p[4],p[5],p[6],p[7],p[8],p[9],p[10] + + u!(uout, t) # uout = u(t) + + #dx .= A*x + B1*ut + mul!(dx, A, x) + mul!(tmp, B1, uout) + dx .+= tmp + + for k=1:length(Tau) # Add each of the delayed signals + u!(uout, t-Tau[k]) # uout = u(t-tau[k]) + h!(hout, p, t-Tau[k]) + dk_delayed = dot(view(C2,k,:), hout) + dot(view(D21,k,:), uout) + dx .+= view(B2,:, k) .* dk_delayed + end + return +end -function lsim(sys::DelayLtiSystem, t::AbstractArray{<:Real}; u=(t -> fill(0.0, ninputs(sys))), x0=fill(0.0, nstates(sys)), alg=MethodOfSteps(Tsit5()), kwargs...) +function _lsim(sys::DelayLtiSystem{T}, u!, t::AbstractArray{<:T}, x0::Vector{T}, alg) where T P = sys.P if ~iszero(P.D22) @@ -35,39 +93,54 @@ function lsim(sys::DelayLtiSystem, t::AbstractArray{<:Real}; u=(t -> fill(0.0, n error("The t-vector should be uniformly spaced, t[2] - t[1] = $dt.") # Perhaps dedicated function for checking this? end - # Slightly more complicated definition since the d signal is not directly available - dde = function (dx, x, h, p, t) - dx .= P.A*x + P.B1*u(t) - for k=1:length(sys.Tau) # Add each of the delayed signals - dk_delayed = dot(P.C2[k,:], h(p,t-sys.Tau[k])) + dot(P.D21[k,:], u(t-sys.Tau[k])) - dx .+= P.B2[:, k] * dk_delayed - end - end + # Get all matrices to save on allocations + A, B1, B2, C1, C2, D11, D12, D21, D22 = P.A, P.B1, P.B2, P.C1, P.C2, P.D11, P.D12, P.D21, P.D22 + Tau = sys.Tau - h_initial = (p, t) -> zeros(nstates(sys)) + hout = fill(zero(T), nstates(sys)) # in place storage for h + uout = fill(zero(T), ninputs(sys)) # in place storage for u + tmp = similar(x0) - # Saves y (excluding the d(t-τ) contribution) and d - saved_values = SavedValues(Float64, Tuple{Vector{Float64}, Vector{Float64}}) - cb = SavingCallback((x,t,integrator) -> (P.C1*x + P.D11*u(t), P.C2*x + P.D21*u(t)), saved_values, saveat=t) + h!(out, p, t) = (out .= 0) # History function - prob = DDEProblem(dde, x0, h_initial, (0.0, 8.0), constant_lags=sys.Tau) + p = (A, B1, B2, C2, D21, Tau, u!, uout, hout, tmp) + prob = DDEProblem{true}(dde_param, x0, h!, (t[1], t[end]), p, constant_lags=sys.Tau) - sol = solve(prob, alg, callback=cb; saveat=t, kwargs...) + sol = DelayDiffEq.solve(prob, alg, saveat=t) - x = sol.u # the states are labeled u in DifferentialEquations - y = hcat([saved_values.saveval[k][1] for k=1:length(t)]...) - d = hcat([saved_values.saveval[k][2] for k=1:length(t)]...) + x = sol.u::Array{Array{T,1},1} # the states are labeled u in DelayDiffEq + println(size(x)) - # Account for the effect of the delayed d-signal on y - for k=1:length(sys.Tau) - N_del = Integer(sys.Tau[k] / dt) - dk = [zeros(N_del); d[k, 1:end-N_del]] + y = Array{T,2}(undef, noutputs(sys), length(t)) + d = Array{T,2}(undef, size(C2,1), length(t)) + # Build y signal (without d term) + for k = 1:length(t) + u!(uout, t[k]) + y[:,k] = C1*x[k] + D11*uout + #d[:,k] = C2*x[k] + D21*uout + end + xitp = Interpolations.interpolate((t,), x, Interpolations.Gridded(Interpolations.Linear())) + + dtmp = Array{T,1}(undef, size(C2,1)) + # Function to evaluate d(t)_i at an arbitrary time + # X is constinuous, so interpoate, u is not + function dfunc!(tmp::Array{T1}, t, i) where T1 + tmp .= if t < 0 + T1(0) + else + xitp(t) + end + u!(uout, t) + return dot(view(C2,i,:),tmp) + dot(view(D21,i,:),uout) + end + # Account for the effect of the delayed d-signal on y + for k=1:length(Tau) for j=1:length(t) - y[:, j] .+= sys.P.D12[:, k] * dk[j] + di = dfunc!(tmp, t[j] - Tau[k], k) + y[:, j] .+= view(D12,:, k) .* di end end - - t, x, y + return t, x, y end diff --git a/src/types/PartionedStateSpace.jl b/src/types/PartionedStateSpace.jl index d3c954ae6..b42294c66 100644 --- a/src/types/PartionedStateSpace.jl +++ b/src/types/PartionedStateSpace.jl @@ -60,7 +60,7 @@ function blkdiag(A1::Matrix{T1}, A2::Matrix{T2}) where {T1<:Number, T2<:Number} dims1 = size(A1) dims2 = size(A2) - A_new = zeros(Float64, dims1 .+ dims2) + A_new = zeros(T, dims1 .+ dims2) A_new[1:dims1[1], 1:dims1[2]] = A1 A_new[dims1[1]+1:end, dims1[2]+1:end] = A2 From eca65985586aebc886671acab290018f489685bb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mattias=20F=C3=A4lt?= Date: Sat, 2 Feb 2019 16:00:18 +0100 Subject: [PATCH 20/30] Working version of delay systems --- example/delayed_lti_timeresp.jl | 72 ++++++++++++++++--- src/connections.jl | 25 +++++++ src/delay_systems.jl | 58 +++++++++++++-- src/plotting.jl | 14 ++++ src/types/DelayLtiSystem.jl | 80 +++++++++++++++++++-- src/types/Lti.jl | 26 +++++++ src/types/PartionedStateSpace.jl | 117 ++++++++++++++++++++++++------- src/types/TransferFunction.jl | 1 - src/types/conversion.jl | 8 ++- src/types/promotion.jl | 15 ++++ test/test_delayed_systems.jl | 56 +++++++++++++-- 11 files changed, 419 insertions(+), 53 deletions(-) diff --git a/example/delayed_lti_timeresp.jl b/example/delayed_lti_timeresp.jl index 4b4a46681..0753c3480 100644 --- a/example/delayed_lti_timeresp.jl +++ b/example/delayed_lti_timeresp.jl @@ -1,13 +1,14 @@ -using Plots -plotly() +using ControlSystems, Plots +gr() sys = feedback(1.0, ss(-1.0, 2, 1, 0) * (delay(2.0) + delay(3.0) + delay(2.5))) +sys = feedback(ss(-1.0, 1, 1, 0), delay(1.0)) t = 0:0.02:8 -@time lsim(sys, t-> [t>=0 ? 1.0 : 0.0], t) -@time t, x, y = lsim(sys, [1.0], t) -@time t, x, y = lsim(sys, (out, t) -> (out .= (t>=0 ? 1.0 : 0.0)), t) -@time t, x, y = lsim(sys, (out, t) -> (out[1] = (t>=0 ? 1.0 : 0.0)), t) +@time y, t, x = lsim(sys, t-> [t>=0 ? 1.0 : 0.0], t) +@time y, t, x = lsim(sys, [1.0], t) +@time y, t, x = lsim(sys, (out, t) -> (out .= (t>=0 ? 1.0 : 0.0)), t) +@time y, t, x = lsim(sys, (out, t) -> (out[1] = (t>=0 ? 1.0 : 0.0)), t) function u0(out,t) if t > 0 @@ -18,9 +19,62 @@ function u0(out,t) return end -@time t, x, y = lsim(sys, t; u=u0) +@time y, t, x = lsim(sys, u0, t) plot(t, y') -gui() -@time ControlSystems._lsim(sys, t, (out,t)-> out[1] = (t>=0 ? 1.0 : 0.0)) +s = tf("s") +P = delay(2.6)*ss((s+3.0)/(s^2+0.3*s+1)) +C = 0.06 * ss(1.0 + 1/s); +P*C +T = feedback(P*C,1.0) + +t = 0:0.1:70 +y, t, x = lsim(T, t -> (t<0 ? 0 : 1 ), t) +plot(t, y, c = :blue) + +w = 10 .^ (-2:0.01:2) +marginplot(P*C, w) +marginplot(P*C) + +notch = ss(tf([1, 0.2, 1],[1, .8, 1])); +C = ss(0.05 * (1 + 1/s)); +Tnotch = feedback(P*C*notch, 1.0) + +stepplot(Tnotch) + +y, t, x = step(C, method=:zoh) + +y2, t2, x2 = step(Tnotch) +stepplot(Tnotch) + +stepplot(Tnotch, 40, 0.1) + +stepplot(T, 100) + +G = delay(5)/(s+1) +T = feedback(G, 0.5) +w = 10 .^ (-2:0.01:3) +bodeplot(T, w, plotphase=false) + +# Test conversion, promotion +delay(1,Int64) + 3.5 + +G = 1 + 0.5 * delay(3) +w = 10 .^(-2:0.001:2) +bodeplot(G, w, plotphase=false) + +G = delay(1) * ((0.8*s^2+s+2)/(s^2+s)) +T = feedback(G,1) +# Not possible with direct term +stepplot(T) + +bodeplot(T) + +G = 1/(s+1) + delay(4) +T = feedback(1,G) +# Not possible to lsim with direct term +stepplot(T) +bodeplot(T) + +s = tf("s") diff --git a/src/connections.jl b/src/connections.jl index ae2491254..c7a000623 100644 --- a/src/connections.jl +++ b/src/connections.jl @@ -45,6 +45,25 @@ end append(systems::LTISystem...) = append(promote(systems...)...) function Base.vcat(systems::ST...) where ST <: AbstractStateSpace +# TODO Move size check to wrappers + +function Base.vcat(systems::DelayLtiSystem...) + for sys in systems + println(sys.P.ny1) + end + P = vcat_1([sys.P for sys in systems]...) # See PartitionedStateSpace + Tau = vcat([sys.Tau for sys in systems]...) + return DelayLtiSystem(P, Tau) +end + +function Base.hcat(systems::DelayLtiSystem...) + P = hcat_1([sys.P for sys in systems]...) # See PartitionedStateSpace + Tau = vcat([sys.Tau for sys in systems]...) + return DelayLtiSystem(P, Tau) +end + + +function Base.vcat(systems::StateSpace...) # Perform checks nu = systems[1].nu if !all(s.nu == nu for s in systems) @@ -125,6 +144,12 @@ Base.typed_hcat(::Type{T}, X...) where {T<:LTISystem} = hcat(convert.(T, X)...) # Ambiguity Base.typed_hcat(::Type{T}, X::Number...) where {T<:LTISystem, N} = hcat(convert.(T, X)...) +# Catch special cases where inv(sys) might not be possible after promotion, like improper tf +function /(sys1::Union{StateSpace,DelayLtiSystem}, sys2::LTISystem) + sys1new, sys2new = promote(sys1, 1/sys2) + return sys1new*sys2new +end + # function hvcat(rows::Tuple{Vararg{Int}}, systems::Union{Number,AbstractVecOrMat{<:Number},LTISystem}...) # T = Base.promote_typeof(systems...) # nbr = length(rows) # number of block rows diff --git a/src/delay_systems.jl b/src/delay_systems.jl index a530e8fde..5a4a5d609 100644 --- a/src/delay_systems.jl +++ b/src/delay_systems.jl @@ -33,7 +33,7 @@ end """ - `t, x, y = lsim(sys::DelayLtiSystem, t::AbstractArray{<:Real}; u=(out, t) -> (out .= 0), x0=fill(0.0, nstates(sys)), alg=MethodOfSteps(Tsit5()), kwargs...)` + `y, t, x = lsim(sys::DelayLtiSystem, t::AbstractArray{<:Real}; u=(out, t) -> (out .= 0), x0=fill(0.0, nstates(sys)), alg=MethodOfSteps(Tsit5()), kwargs...)` Simulate system `sys`, over time `t`, using input signal `u`, with initial state `x0`, using method `alg` . @@ -45,7 +45,7 @@ end Function `ut .= u(t)`, or In-place function `u(ut, t)`. (Slightly more effienct) - Returns: `x` and `y` at times `t`. + Returns: times `t`, and `y` and `x` at those times. """ function lsim(sys::DelayLtiSystem{T}, u, t::AbstractArray{<:T}; x0=fill(zero(T), nstates(sys)), alg=MethodOfSteps(Tsit5())) where T # Make u! in-place function of u @@ -88,6 +88,7 @@ function _lsim(sys::DelayLtiSystem{T}, u!, t::AbstractArray{<:T}, x0::Vector{T}, error("non-zero D22-matrix block is not supported") # Due to limitations in differential equations end + t0 = first(t) dt = t[2] - t[1] if ~all(diff(t) .≈ dt) # QUESTION Does this work or are there precision problems? error("The t-vector should be uniformly spaced, t[2] - t[1] = $dt.") # Perhaps dedicated function for checking this? @@ -125,7 +126,7 @@ function _lsim(sys::DelayLtiSystem{T}, u!, t::AbstractArray{<:T}, x0::Vector{T}, # Function to evaluate d(t)_i at an arbitrary time # X is constinuous, so interpoate, u is not function dfunc!(tmp::Array{T1}, t, i) where T1 - tmp .= if t < 0 + tmp .= if t < t0 T1(0) else xitp(t) @@ -142,5 +143,54 @@ function _lsim(sys::DelayLtiSystem{T}, u!, t::AbstractArray{<:T}, x0::Vector{T}, end end - return t, x, y + return y', t, hcat(x...)' +end + + +# We have to default to something, look at the sys.P.P and delays +function _bounds_and_features(sys::DelayLtiSystem, plot::Symbol) + ws, pz = _bounds_and_features(sys.P.P, plot) + logtau = log10.(abs.(sys.Tau)) + logtau = logtau[logtau .> -4] # Ignore low frequency + if isempty(logtau) + return ws, pz + end + extreme = extrema(logtau) + return [min(ws[1], floor(extreme[1]-0.2)), max(ws[2], ceil(extreme[2]+0.2))], pz +end + +# Againm we have to do something for default vectors, more or less a copy from timeresp.jl +function _default_Ts(sys::DelayLtiSystem) + if !isstable(sys.P.P) + return 0.05 # Something small + else + ps = pole(sys.P.P) + r = minimum([abs.(real.(ps));0]) # Find the fastest pole of sys.P.P + r = min(r, minimum([sys.Tau;0])) # Find the fastest delay + if r == 0.0 + r = 1.0 + end + return 0.07/r + end +end + +iscontinuous(sys::DelayLtiSystem) = true + +function Base.step(sys::DelayLtiSystem{T}, t::AbstractVector, kwargs...) where T + nu = ninputs(sys) + if t[1] != 0 + throw(ArgumentError("First time point must be 0 in step")) + end + u = (out, t) -> (t < 0 ? out .= 0 : out .= 1) + x0=fill(zero(T), nstates(sys)) + if nu == 1 + y, tout, x = lsim(sys, u, t, x0=x0, kwargs...) + else + x = Array{T}(undef, length(t), nstates(sys), nu) + y = Array{T}(undef, length(t), noutputs(sys), nu) + for i=1:nu + y[:,:,i], tout, x[:,:,i] = lsim(sys[:,i], u, t, x0=x0, kwargs...) + end + end + return y, tout, x end diff --git a/src/plotting.jl b/src/plotting.jl index 1beb556f7..00c5344b9 100644 --- a/src/plotting.jl +++ b/src/plotting.jl @@ -647,6 +647,20 @@ function marginplot(systems::Union{AbstractVector{T},T}, args...; kwargs...) whe for j=1:nu for i=1:ny wgm, gm, wpm, pm, fullPhase = sisomargin(s[i,j],w, full=true, allMargins=true) + # Let's be reasonable, only plot 5 smallest gain margins + if length(gm) > 5 + @warn "Only showing smallest 5 out of $(length(gm)) gain margins" + idx = sortperm(gm) + wgm = wgm[idx[1:5]] + gm = gm[idx[1:5]] + end + # Let's be reasonable, only plot 5 smallest phase margins + if length(pm) > 5 + @warn "Only showing \"smallest\" 5 out of $(length(pm)) phase margins" + idx = sortperm(pm) + wgm = wpm[idx[1:5]] + gm = pm[idx[1:5]] + end if _PlotScale == "dB" mag = 20 .* log10.(1 ./ gm) oneLine = 0 diff --git a/src/types/DelayLtiSystem.jl b/src/types/DelayLtiSystem.jl index 24e7c9713..aba3880f6 100644 --- a/src/types/DelayLtiSystem.jl +++ b/src/types/DelayLtiSystem.jl @@ -13,7 +13,7 @@ end # QUESTION: would psys be a good standard variable name for a PartionedStateSpace # and perhaps dsys for a delayed system, (ambigous with discrete system though) -function DelayLtiSystem{T}(sys::StateSpace, Tau::Vector{Float64}) where T +function DelayLtiSystem{T}(sys::StateSpace, Tau::Vector{Float64}) where T<:Number nu = ninputs(sys) - length(Tau) ny = noutputs(sys) - length(Tau) @@ -21,24 +21,32 @@ function DelayLtiSystem{T}(sys::StateSpace, Tau::Vector{Float64}) where T error("Time vector is too long.") end - psys = PartionedStateSpace(sys, ny, nu) + psys = PartionedStateSpace{StateSpace{T,Matrix{T}}}(sys, nu, ny) DelayLtiSystem{T}(psys, Tau) end +# For converting DelayLtiSystem{T} to different T +DelayLtiSystem{T}(sys::DelayLtiSystem) where T = DelayLtiSystem{T}(PartionedStateSpace{StateSpace{T,Matrix{T}}}(sys.P), Float64[]) +DelayLtiSystem{T}(sys::StateSpace) where T = DelayLtiSystem{T}(sys, Float64[]) +# From StateSpace, infer type DelayLtiSystem(sys::StateSpace{T,MT}, Tau::Vector{Float64}) where {T, MT} = DelayLtiSystem{T}(sys, Tau) DelayLtiSystem(sys::StateSpace{T,MT}) where {T, MT} = DelayLtiSystem{T}(sys, Float64[]) -DelayLtiSystem{T}(sys::StateSpace) where T = DelayLtiSystem{T}(sys, Float64[]) +# From TransferFunction, infer type TODO Use proper constructor instead of convert here when defined +DelayLtiSystem(sys::TransferFunction{S}) where {T,S<:SisoTf{T}} = DelayLtiSystem(convert(StateSpace{T, Matrix{T}}, sys)) # TODO: Think through these promotions and conversions -Base.promote_rule(::Type{<:StateSpace{T1}}, ::Type{DelayLtiSystem{T2}}) where {T1<:Number,T2<:Number} = DelayLtiSystem{promote_type(T1,T2)} Base.promote_rule(::Type{AbstractMatrix{T1}}, ::Type{DelayLtiSystem{T2}}) where {T1<:Number,T2<:Number} = DelayLtiSystem{promote_type(T1,T2)} Base.promote_rule(::Type{T1}, ::Type{DelayLtiSystem{T2}}) where {T1<:Number,T2<:Number} = DelayLtiSystem{promote_type(T1,T2)} #Base.promote_rule(::Type{<:UniformScaling}, ::Type{S}) where {S<:DelayLtiSystem} = DelayLtiSystem{T} Base.convert(::Type{DelayLtiSystem{T}}, sys::StateSpace) where T = DelayLtiSystem{T}(sys) Base.convert(::Type{DelayLtiSystem{T1}}, d::T2) where {T1,T2 <: Number} = DelayLtiSystem{T1}(ss(d)) -Base.convert(::Type{DelayLtiSystem{T}}, sys::DelayLtiSystem) where T = DelayLtiSystem{T}(StateSpace{T,Matrix{T}}(sys.P.P), sys.Tau) +# Catch convertsion between T +Base.convert(::Type{S}, sys::DelayLtiSystem) where {T, S<:DelayLtiSystem{T}} = + sys isa S ? sys : S(StateSpace{T,Matrix{T}}(sys.P.P), sys.Tau) + +Base.convert(::Type{DelayLtiSystem{T}}, sys::TransferFunction) where T = DelayLtiSystem{T}(convert(StateSpace{T, Matrix{T}}, sys)) function *(sys::DelayLtiSystem, n::Number) @@ -48,10 +56,69 @@ function *(sys::DelayLtiSystem, n::Number) end *(n::Number, sys::DelayLtiSystem) = *(sys, n) +function +(sys::DelayLtiSystem{T1}, n::T2) where {T1,T2<:Number} + T = promote_type(T1,T2) + if T == T1 # T2 can be stored in sys + +(sys, T1(n)) + else # We need to upgrade sys + +(DelayLtiSystem{T}(sys), T(n)) + end +end + +# Efficient addition +function +(sys::DelayLtiSystem{T}, n::T) where {T<:Number} + ny, nu = size(sys) + ssold = sys.P.P + # Add to direct term from input to output + new_D = ssold.D + new_D[1:ny, 1:nu] .+= n + + pnew = PartionedStateSpace(StateSpace(ssold.A, ssold.B, ssold.C, new_D, 0.0), ny, nu) + DelayLtiSystem(pnew, sys.Tau) +end + +# Efficient subtraction with number +-(sys::DelayLtiSystem, n::T) where {T <:Number} = +(sys, -n) +-(n::T, sys::DelayLtiSystem) where {T <:Number} = +(-sys, n) + ninputs(sys::DelayLtiSystem) = size(sys.P.P, 2) - length(sys.Tau) noutputs(sys::DelayLtiSystem) = size(sys.P.P, 1) - length(sys.Tau) nstates(sys::DelayLtiSystem) = nstates(sys.P.P) +Base.size(sys::DelayLtiSystem) = (noutputs(sys), ninputs(sys)) + +# Fallbacks, TODO We should sort this out for all types, maybe after SISO/MIMO +# {Array, Number}, Colon +Base.getindex(sys::DelayLtiSystem, i, ::Colon) = + getindex(sys, index2range(i), 1:size(sys,2)) +# Colon, {Array, Number} +Base.getindex(sys::DelayLtiSystem, ::Colon, j) = + getindex(sys, 1:size(sys,1), index2range(j)) +Base.getindex(sys::DelayLtiSystem, ::Colon, ::Colon) = + getindex(sys, 1:size(sys,1), 1:size(sys,2)) +# Should just catch Number, Number, or Colon, Colon +Base.getindex(sys::DelayLtiSystem, i, j) = + getindex(sys, index2range(i), index2range(j)) + +function Base.getindex(sys::DelayLtiSystem, i::AbstractArray, j::AbstractArray) + ny, nu = size(sys) + # Cant use "boundscheck" since not AbstractArray + imin, imax = extrema(i) + jmin, jmax = extrema(j) + if imax > ny || imin < 1 || jmax > nu || jmin < 1 + throw(BoundsError(sys, (i,j))) + end + nrow, ncol = size(sys.P.P) + rowidx = [collect(i); collect((ny+1):nrow)] # Output plus delay terms + colidx = [collect(j); collect((nu+1):ncol)] # Input plus delay terms + DelayLtiSystem(StateSpace( + sys.P.A[:, :], + sys.P.B[:, colidx], + sys.P.C[rowidx, :], + sys.P.D[rowidx, colidx], + sys.P.Ts), sys.Tau) +end + function +(sys1::DelayLtiSystem, sys2::DelayLtiSystem) psys_new = sys1.P + sys2.P Tau_new = [sys1.Tau; sys2.Tau] @@ -59,6 +126,9 @@ function +(sys1::DelayLtiSystem, sys2::DelayLtiSystem) DelayLtiSystem(psys_new.P, Tau_new) end +-(sys1::DelayLtiSystem, sys2::DelayLtiSystem) = +(sys1, -sys2) +-(sys::DelayLtiSystem{T}) where {T} = *(sys, T(-1)) + function *(sys1::DelayLtiSystem, sys2::DelayLtiSystem) psys_new = sys1.P * sys2.P diff --git a/src/types/Lti.jl b/src/types/Lti.jl index d109aac6c..baa61297f 100644 --- a/src/types/Lti.jl +++ b/src/types/Lti.jl @@ -4,6 +4,29 @@ abstract type LTISystem <: AbstractSystem end *(sys1::LTISystem, sys2::LTISystem) = *(promote(sys1, sys2)...) /(sys1::LTISystem, sys2::LTISystem) = /(promote(sys1, sys2)...) +# Fallback number ++(sys1::LTISystem, sys2::Number) = +(promote(sys1, sys2)...) +-(sys1::LTISystem, sys2::Number) = -(promote(sys1, sys2)...) +*(sys1::LTISystem, sys2::Number) = *(promote(sys1, sys2)...) +/(sys1::LTISystem, sys2::Number) = /(promote(sys1, sys2)...) + ++(sys1::Number, sys2::LTISystem) = +(promote(sys1, sys2)...) +-(sys1::Number, sys2::LTISystem) = -(promote(sys1, sys2)...) +*(sys1::Number, sys2::LTISystem) = *(promote(sys1, sys2)...) +/(sys1::Number, sys2::LTISystem) = /(promote(sys1, sys2)...) + +# Fallback Matrix ++(sys1::LTISystem, sys2::AbstractMatrix) = +(promote(sys1, sys2)...) +-(sys1::LTISystem, sys2::AbstractMatrix) = -(promote(sys1, sys2)...) +*(sys1::LTISystem, sys2::AbstractMatrix) = *(promote(sys1, sys2)...) +/(sys1::LTISystem, sys2::AbstractMatrix) = /(promote(sys1, sys2)...) + ++(sys1::AbstractMatrix, sys2::LTISystem) = +(promote(sys1, sys2)...) +-(sys1::AbstractMatrix, sys2::LTISystem) = -(promote(sys1, sys2)...) +*(sys1::AbstractMatrix, sys2::LTISystem) = *(promote(sys1, sys2)...) +/(sys1::AbstractMatrix, sys2::LTISystem) = /(promote(sys1, sys2)...) + +# TODO We should have proper fallbacks for matrices too feedback(sys1::Union{LTISystem,Number,AbstractMatrix{<:Number}}, sys2::Union{LTISystem,Number,AbstractMatrix{<:Number}}) = feedback(promote(sys1, sys2)...) @@ -57,3 +80,6 @@ function _check_consistent_sampling_time(sys1::LTISystem, sys2::LTISystem) error("Sampling time mismatch") end end + +# Fallback since LTISystem not AbstractArray +Base.size(sys::LTISystem, i::Integer) = size(sys)[i] diff --git a/src/types/PartionedStateSpace.jl b/src/types/PartionedStateSpace.jl index b42294c66..5a3c63589 100644 --- a/src/types/PartionedStateSpace.jl +++ b/src/types/PartionedStateSpace.jl @@ -16,7 +16,9 @@ struct PartionedStateSpace{S} nu1::Int ny1::Int end - +# For converting between different S +PartionedStateSpace{S}(partsys::PartionedStateSpace) where {S<:StateSpace} = + PartionedStateSpace{S}(S(partsys.P), partsys.nu1, partsys.ny1) function getproperty(sys::PartionedStateSpace, d::Symbol) P = getfield(sys, :P) @@ -52,32 +54,16 @@ function getproperty(sys::PartionedStateSpace, d::Symbol) end end - -# There should already exist a function like this somewhare? -function blkdiag(A1::Matrix{T1}, A2::Matrix{T2}) where {T1<:Number, T2<:Number} - T = promote_type(T1, T2) - - dims1 = size(A1) - dims2 = size(A2) - - A_new = zeros(T, dims1 .+ dims2) - A_new[1:dims1[1], 1:dims1[2]] = A1 - A_new[dims1[1]+1:end, dims1[2]+1:end] = A2 - - return A_new -end - - function +(s1::PartionedStateSpace, s2::PartionedStateSpace) - A = blkdiag(s1.A, s2.A) + A = blockdiag(s1.A, s2.A) - B = [[s1.B1; s2.B1] blkdiag(s1.B2, s2.B2)] + B = [[s1.B1; s2.B1] blockdiag(s1.B2, s2.B2)] C = [[s1.C1 s2.C1]; - blkdiag(s1.C2, s2.C2)] + blockdiag(s1.C2, s2.C2)] D = [(s1.D11 + s2.D11) s1.D12 s2.D12; - [s1.D21; s2.D21] blkdiag(s1.D22, s2.D22)] + [s1.D21; s2.D21] blockdiag(s1.D22, s2.D22)] P = StateSpace(A, B, C, D, 0) # How to handle discrete? PartionedStateSpace(P, s1.nu1 + s2.nu1, s1.ny1 + s2.ny1) @@ -122,35 +108,112 @@ function feedback(s1::PartionedStateSpace, s2::PartionedStateSpace) X_12 = (I + s2.D11*s1.D11)\[I -s2.D11*s1.D12 -s2.D12] X_22 = (I + s1.D11*s2.D11)\[s1.D11 s1.D12 -s1.D11*s2.D12] - A = [s1.B1 * X_11 ; s2.B1 * X_21] + blkdiag(s1.A, s2.A) + A = [s1.B1 * X_11 ; s2.B1 * X_21] + blockdiag(s1.A, s2.A) B = [s1.B1 * X_12 ; s2.B1 * X_22] - tmp = blkdiag(s1.B2, s2.B2) + tmp = blockdiag(s1.B2, s2.B2) B[:, end-size(tmp,2)+1:end] .+= tmp C = [s1.D11 * X_11 ; s1.D21 * X_11 ; - s2.D21 * X_21 ] + [s1.C1 zeros(size(s1.C1,1),size(s2.C1,2)); blkdiag(s1.C2, s2.C2)] + s2.D21 * X_21 ] + [s1.C1 zeros(size(s1.C1,1),size(s2.C1,2)); blockdiag(s1.C2, s2.C2)] D = [s1.D11 * X_12 ; s1.D21 * X_12 ; s2.D21 * X_22 ] - tmp = [s1.D12 zeros(size(s1.D12,1),size(s2.D12,2)); blkdiag(s1.D22, s2.D22)] + tmp = [s1.D12 zeros(size(s1.D12,1),size(s2.D12,2)); blockdiag(s1.D22, s2.D22)] D[:, end-size(tmp,2)+1:end] .+= tmp # in case it is desired to consider both outputs # C = [s1.D11 * X_11 ; # s2.D11 * X_21 ; # s1.D21 * X_11 ; - # s2.D21 * X_21 ] + [blkdiag(s1.C1, s2.C1); blkdiag(s1.C2, s2.C2)] + # s2.D21 * X_21 ] + [blockdiag(s1.C1, s2.C1); blockdiag(s1.C2, s2.C2)] # # D = [s1.D11 * X_12 ; # s2.D11 * X_22 ; # s1.D21 * X_12 ; # s2.D21 * X_22 ] - #tmp = [blkdiag(s1.D12, s2.D12); blkdiag(s1.D22, s2.D22)] + #tmp = [blockdiag(s1.D12, s2.D12); blockdiag(s1.D22, s2.D22)] #D[:, end-size(tmp,2)+1:end] .+= tmp P = StateSpace(A, B, C, D, 0) PartionedStateSpace(P, s2.nu1, s1.ny1) end + +""" Concatenate systems vertically with + same first input u1 + second input [u2_1; u2_2 ...] + and output y1 = [y1_1; y1_2, ...] + y2 = [y2_1; y2_2, ...] + for u1_i, u2_i, y1_i, y2_i, where i denotes system i +""" +function vcat_1(systems::PartionedStateSpace...) + # Perform checks + println("vcal $(length(systems))") + println(systems) + Ts = systems[1].P.Ts + if !all(s.P.Ts == Ts for s in systems) + error("All systems have same sample time") + end + nu1 = systems[1].nu1 + if !all(s.nu1 == nu1 for s in systems) + error("All systems must have same first input dimension") + end + + A = blockdiag([s.A for s in systems]...) + + B1 = vcat([s.B1 for s in systems]...) + B2 = blockdiag([s.B2 for s in systems]...) + for i = 1:length(systems) + println(systems[i].ny1) + end + println("after") + C1 = blockdiag([s.C1 for s in systems]...) + C2 = blockdiag([s.C2 for s in systems]...) + + D11 = vcat([s.D11 for s in systems]...) + D12 = blockdiag([s.D12 for s in systems]...) + D21 = vcat([s.D21 for s in systems]...) + D22 = blockdiag([s.D22 for s in systems]...) + + sysnew = StateSpace(A, [B1 B2], [C1; C2], [D11 D12; D21 D22], Ts) + return PartionedStateSpace(sysnew, nu1, sum(s -> s.ny1, systems)) +end + + +""" Concatenate systems horizontally with + same first ouput y1 being the sum + second output y2 = [y2_1; y2_2, ...] + and inputs u1 = [u1_1; u1_2, ...] + u2 = [u2_1; u2_2, ...] + for u1_i, u2_i, y1_i, y2_i, where i denotes system i +""" +function hcat_1(systems::PartionedStateSpace...) + println("hcat $(length(systems))") + # Perform checks + Ts = systems[1].P.Ts + if !all(s.P.Ts == Ts for s in systems) + error("All systems have same sample time") + end + ny1 = systems[1].ny1 + if !all(s.ny1 == ny1 for s in systems) + error("All systems must have same first ouput dimension") + end + + A = blockdiag([s.A for s in systems]...) + + B1 = blockdiag([s.B1 for s in systems]...) + B2 = blockdiag([s.B2 for s in systems]...) + + C1 = hcat([s.C1 for s in systems]...) + C2 = blockdiag([s.C2 for s in systems]...) + + D11 = hcat([s.D11 for s in systems]...) + D12 = hcat([s.D12 for s in systems]...) + D21 = blockdiag([s.D21 for s in systems]...) + D22 = blockdiag([s.D22 for s in systems]...) + + sysnew = StateSpace(A, [B1 B2], [C1; C2], [D11 D12; D21 D22], Ts) + return PartionedStateSpace(sysnew, sum(s -> s.nu1, systems), ny1) +end diff --git a/src/types/TransferFunction.jl b/src/types/TransferFunction.jl index 6df17feb4..5c91ed141 100644 --- a/src/types/TransferFunction.jl +++ b/src/types/TransferFunction.jl @@ -44,7 +44,6 @@ ninputs(G::TransferFunction) = size(G.matrix, 2) ## INDEXING ## Base.ndims(::TransferFunction) = 2 Base.size(G::TransferFunction) = size(G.matrix) -Base.size(G::TransferFunction, d) = size(G.matrix, d) Base.eltype(::Type{S}) where {S<:TransferFunction} = S function Base.getindex(G::TransferFunction{S}, inds...) where {S<:SisoTf} diff --git a/src/types/conversion.jl b/src/types/conversion.jl index 8024a40a3..1497c3cd1 100644 --- a/src/types/conversion.jl +++ b/src/types/conversion.jl @@ -54,8 +54,12 @@ function convert(::Type{TransferFunction{S}}, G::TransferFunction) where S return TransferFunction{eltype(Gnew_matrix)}(Gnew_matrix, G.Ts) end -function convert(::Type{StateSpace{T,MT}}, sys::StateSpace) where {T, MT} - return StateSpace{T,MT}(convert(MT, sys.A), convert(MT, sys.B), convert(MT, sys.C), convert(MT, sys.D), sys.Ts) +function convert(::Type{S}, sys::StateSpace) where {T, MT, S <:StateSpace{T,MT}} + if sys isa S + return sys + else + return StateSpace{T,MT}(convert(MT, sys.A), convert(MT, sys.B), convert(MT, sys.C), convert(MT, sys.D), sys.Ts) + end end Base.convert(::Type{HeteroStateSpace{AT,BT,CT,DT}}, s::StateSpace{T,MT}) where {T,MT,AT,BT,CT,DT} = HeteroStateSpace{promote_type(MT,AT),promote_type(MT,BT),promote_type(MT,CT),promote_type(MT,DT)}(s.A,s.B,s.C,s.D,s.Ts) diff --git a/src/types/promotion.jl b/src/types/promotion.jl index ee0caa040..e86c0cf27 100644 --- a/src/types/promotion.jl +++ b/src/types/promotion.jl @@ -43,6 +43,13 @@ function Base.promote_rule(::Type{TransferFunction{S1}}, ::Type{StateSpace{T2,MT end # NOTE: Perhaps should try to keep matrix structure? +function Base.promote_rule(::Type{TransferFunction{S1}}, ::Type{DelayLtiSystem{T2}}) where {T1,S1<:SisoTf{T1},T2} + DelayLtiSystem{promote_type(T1,T2)} +end +function Base.promote_rule(::Type{StateSpace{T1,MT}}, ::Type{DelayLtiSystem{T2}}) where {T1,MT,T2} + DelayLtiSystem{promote_type(T1,T2)} +end + Base.promote_rule(::Type{TransferFunction{S1}}, ::Type{TransferFunction{S2}}) where {S1, S2} = TransferFunction{promote_type(S1, S2)} #Base.promote_rule(::Type{SisoTf}, ::Type{TransferFunction}) = TransferFunction #Base.promote_rule(::Type{SisoZpk}, ::Type{TransferFunction}) = TransferFunction @@ -72,6 +79,14 @@ Base.promote_rule(::Type{TransferFunction{SisoZpk{T1,TR1}}}, ::Type{M2}) where { Base.promote_rule(::Type{TransferFunction{SisoRational{T1}}}, ::Type{M2}) where {T1, T2, M2<:AbstractMatrix{T2}} = TransferFunction{SisoRational{promote_type(T1, T2)}} +function Base.promote_rule(::Type{StateSpace{T1, MT1}}, ::Type{MT2}) where {T1, MT1, MT2<:AbstractMatrix} + MT = promote_type(MT1, MT2) + StateSpace{eltype(MT), MT} +end + +Base.promote_rule(::Type{DelayLtiSystem{T1}}, ::Type{MT1}) where {T1, MT1<:AbstractMatrix} = + DelayLtiSystem{promote_type(T1, eltype(MT1))} + #Base.promote_rule{S<:TransferFunction{<:SisoTf}}(::Type{S}, ::Type{<:Real}) = S # We want this, but not possible, so hardcode for SisoTypes diff --git a/test/test_delayed_systems.jl b/test/test_delayed_systems.jl index 54422e0d2..3a41e1b98 100644 --- a/test/test_delayed_systems.jl +++ b/test/test_delayed_systems.jl @@ -5,7 +5,7 @@ @test typeof(promote(delay(0.2), ss(1.0 + im))[1]) == DelayLtiSystem{Complex{Float64}} # Extremely baseic tests -@test freqresp(delay(1), ω)[:] ≈ exp.(-im*ω) rtol=1e-15 +@test freqresp(delay(1), ω) ≈ reshape(exp.(-im*ω), length(ω), 1, 1) rtol=1e-15 @test freqresp(delay(2.5), ω)[:] ≈ exp.(-2.5im*ω) rtol=1e-15 @test freqresp(3.5*delay(2.5), ω)[:] ≈ 3.5*exp.(-2.5im*ω) rtol=1e-15 @test freqresp(delay(2.5)*1.5, ω)[:] ≈ exp.(-2.5im*ω)*1.5 rtol=1e-15 @@ -22,10 +22,11 @@ P2_fr = (im*ω .+ 1) ./ (im*ω .+ 2) ## Addition -@test_broken freqresp(1 + delay(1), ω)[:] ≈ 1 .+ exp.(-im*ω) # FIXME; Add suitable conversion from Int64 to DelayLtiSystem +@test freqresp(1 + delay(1), ω)[:] ≈ 1 .+ exp.(-im*ω) # FIXME; Add suitable conversion from Int64 to DelayLtiSystem @test freqresp(P1 + delay(1), ω)[:] ≈ P1_fr .+ exp.(-im*ω) -#FIXME: the following gives a crash.. freqresp(P1 - delay(1), ω)[:] ≈ P1_fr .+ exp.(-im*ω) +# Substraction +@test freqresp(P1 - delay(1), ω)[:] ≈ P1_fr .- exp.(-im*ω) ## Multiplication @@ -50,14 +51,14 @@ P2_fr = (im*ω .+ 1) ./ (im*ω .+ 2) @test freqresp(feedback(delay(2.0), 0.5), ω) ≈ exp.(-2im*ω) ./ (1 .+ 0.5*exp.(-2im*ω)) @test freqresp(feedback(P1, delay(1)), ω)[:] ≈ P1_fr ./ (1 .+ exp.(-im*ω) .* P1_fr) rtol=1e-15 -@test freqresp(feedback(delay(1), P1), ω)[:] ≈ exp.(-im*ω) ./ (1 .+ exp.(-im*ω) .* P1_fr) #FIXME: Answer is Inf, but should give error.. rtol=1e-15 +@test freqresp(feedback(delay(1), P1), ω)[:] ≈ exp.(-im*ω) ./ (1 .+ exp.(-im*ω) .* P1_fr) rtol=1e-15 #FIXME: Answer is Inf, but should give error.. rtol=1e-15 @test freqresp(feedback(P1*delay(1), 1.0), ω)[:] ≈ P1_fr .* exp.(-im*ω) ./ (1 .+ exp.(-im*ω) .* P1_fr) rtol=1e-15 @test freqresp(feedback(1.0, P1*delay(1)), ω)[:] ≈ 1 ./ (1 .+ exp.(-im*ω) .* P1_fr) rtol=1e-15 @test freqresp(feedback(1.0, P2*0.5*(ss(1.0) + delay(2))), ω)[:] ≈ 1 ./(1 .+ P2_fr .* 0.5.*(1 .+ exp.(-2*im*ω))) -@test_broken freqresp(1.0 + delay(2), ω)[:] # TODO: Add addition for DelayLtiSystem. how to do this in a convenient ammner? +@test freqresp(1.0 + delay(2), ω)[:] ≈ 1 .+ exp.(-2im*ω) rtol=1e-15 G = ss(0.5) + 0.5*delay(2)# + 0.5*delay(3) G_fr = 0.5 .+ 0.5*exp.(-2*im*ω)# + 0.5*exp.(-3*im*ω) @@ -68,6 +69,11 @@ G_fr = 0.5 .+ 0.5*exp.(-2*im*ω)# + 0.5*exp.(-3*im*ω) @test freqresp(feedback(P2, G), ω)[:] ≈ P2_fr ./(1 .+ P2_fr .* G_fr) rtol=1e-15 +# Random conversions +sys1 = DelayLtiSystem(1.0/s) +@test sys1.P.A == sys1.P.D == fill(0,1,1) +@test sys1.P.B*sys1.P.C == fill(1,1,1) +@test sys1.Tau == [] ## Multiple Delays G = ss(1.0) + delay(2) + delay(3) @@ -83,4 +89,44 @@ expected_sys_fr = 1.0 ./ (P1_fr .* (exp.(-2*im*ω) + exp.(-3*im*ω)) .+ 1) @test freqresp(feedback(1.0*P1, G*G*P2*P2), ω)[:] ≈ P1_fr ./(1 .+ (G_fr.^2).*P1_fr.*P2_fr.^2) rtol=1e-15# Somewhat pathological system though +s = tf("s") +s11 = feedback(ss(1/s), delay(1)) +s12 = ss(1/s) +s21 = DelayLtiSystem(ss(1/(s+1))) +s22 = ss(1/(s+10)) + +s1 = [s11 s12] +s2 = [s21 s22] + +f1 = [s1;s2] +f2 = [s11 s12; + s21 s22] + +# Test that different consatenations work +w = 10 .^ (-2:0.1:2) +@test freqresp(f1, w) ≈ freqresp(f2, w) rtol=1e-15 + +# This used to be a weird bug +@test freqresp(s11, w) ≈ freqresp(f2[1,1], w) rtol=1e-15 + + #FIXME: A lot more tests, including MIMO systems in particular + +# Test step +y1, t1, x1 = step([s11;s12], 10) +@test y1[:,2] ≈ step(s12, t1)[1] rtol = 1e-14 + +t = 0.0:0.1:10 +y2, t2, x2 = step(s1, t) +# TODO Figure out which is inexact here +@test y2[:,1,1:1] + y2[:,1,2:2] ≈ step(s11, t)[1] + step(s12, t)[1] rtol=1e-5 + +y3, t3, x3 = step([s11; s12], t) +@test y3[:,1,1] ≈ step(s11, t)[1] rtol = 1e-4 +@test y3[:,2,1] ≈ step(s12, t)[1] rtol = 1e-14 + +y1, t1, x1 = step(DelayLtiSystem([1.0/s 2/s; 3/s 4/s]), t) +y2, t2, x2 = step([1.0/s 2/s; 3/s 4/s], t) +@test y1 ≈ y2 rtol=1e-15 +@test size(x1,1) == length(t) +@test size(x1,3) == 2 From e94ca791e51f348591bd130a1518fc448882e0d4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mattias=20F=C3=A4lt?= Date: Sat, 2 Feb 2019 16:08:26 +0100 Subject: [PATCH 21/30] Minor bug in test, and removed prints --- src/connections.jl | 3 --- src/delay_systems.jl | 4 ---- src/types/PartionedStateSpace.jl | 8 +------- test/test_delayed_systems.jl | 3 +-- 4 files changed, 2 insertions(+), 16 deletions(-) diff --git a/src/connections.jl b/src/connections.jl index c7a000623..5cc918379 100644 --- a/src/connections.jl +++ b/src/connections.jl @@ -48,9 +48,6 @@ function Base.vcat(systems::ST...) where ST <: AbstractStateSpace # TODO Move size check to wrappers function Base.vcat(systems::DelayLtiSystem...) - for sys in systems - println(sys.P.ny1) - end P = vcat_1([sys.P for sys in systems]...) # See PartitionedStateSpace Tau = vcat([sys.Tau for sys in systems]...) return DelayLtiSystem(P, Tau) diff --git a/src/delay_systems.jl b/src/delay_systems.jl index 5a4a5d609..64eff4c65 100644 --- a/src/delay_systems.jl +++ b/src/delay_systems.jl @@ -50,13 +50,10 @@ end function lsim(sys::DelayLtiSystem{T}, u, t::AbstractArray{<:T}; x0=fill(zero(T), nstates(sys)), alg=MethodOfSteps(Tsit5())) where T # Make u! in-place function of u u! = if isa(u, Number) || isa(u,AbstractVector) # Allow for u to be a constant number or vector - println("Number vector") (uout, t) -> uout .= u elseif DiffEqBase.isinplace(u, 2) # If u is an inplace (more than 1 argument function) - println("Inplace") u else # If u is a regular u(t) function - println("Outplace") (out, t) -> (out .= u(t)) end _lsim(sys, UWrapper(u!), t, x0, alg) @@ -110,7 +107,6 @@ function _lsim(sys::DelayLtiSystem{T}, u!, t::AbstractArray{<:T}, x0::Vector{T}, sol = DelayDiffEq.solve(prob, alg, saveat=t) x = sol.u::Array{Array{T,1},1} # the states are labeled u in DelayDiffEq - println(size(x)) y = Array{T,2}(undef, noutputs(sys), length(t)) d = Array{T,2}(undef, size(C2,1), length(t)) diff --git a/src/types/PartionedStateSpace.jl b/src/types/PartionedStateSpace.jl index 5a3c63589..67c76b167 100644 --- a/src/types/PartionedStateSpace.jl +++ b/src/types/PartionedStateSpace.jl @@ -150,8 +150,6 @@ end """ function vcat_1(systems::PartionedStateSpace...) # Perform checks - println("vcal $(length(systems))") - println(systems) Ts = systems[1].P.Ts if !all(s.P.Ts == Ts for s in systems) error("All systems have same sample time") @@ -165,10 +163,7 @@ function vcat_1(systems::PartionedStateSpace...) B1 = vcat([s.B1 for s in systems]...) B2 = blockdiag([s.B2 for s in systems]...) - for i = 1:length(systems) - println(systems[i].ny1) - end - println("after") + C1 = blockdiag([s.C1 for s in systems]...) C2 = blockdiag([s.C2 for s in systems]...) @@ -190,7 +185,6 @@ end for u1_i, u2_i, y1_i, y2_i, where i denotes system i """ function hcat_1(systems::PartionedStateSpace...) - println("hcat $(length(systems))") # Perform checks Ts = systems[1].P.Ts if !all(s.P.Ts == Ts for s in systems) diff --git a/test/test_delayed_systems.jl b/test/test_delayed_systems.jl index 3a41e1b98..735bf1025 100644 --- a/test/test_delayed_systems.jl +++ b/test/test_delayed_systems.jl @@ -69,6 +69,7 @@ G_fr = 0.5 .+ 0.5*exp.(-2*im*ω)# + 0.5*exp.(-3*im*ω) @test freqresp(feedback(P2, G), ω)[:] ≈ P2_fr ./(1 .+ P2_fr .* G_fr) rtol=1e-15 +s = tf("s") # Random conversions sys1 = DelayLtiSystem(1.0/s) @test sys1.P.A == sys1.P.D == fill(0,1,1) @@ -88,8 +89,6 @@ expected_sys_fr = 1.0 ./ (P1_fr .* (exp.(-2*im*ω) + exp.(-3*im*ω)) .+ 1) @test freqresp(feedback(1.0*P1, G*G*P2*P2), ω)[:] ≈ P1_fr ./(1 .+ (G_fr.^2).*P1_fr.*P2_fr.^2) rtol=1e-15# Somewhat pathological system though - -s = tf("s") s11 = feedback(ss(1/s), delay(1)) s12 = ss(1/s) s21 = DelayLtiSystem(ss(1/(s+1))) From d64468ee52094d261099a883278afb01b4f47a66 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mattias=20F=C3=A4lt?= Date: Mon, 25 Mar 2019 20:09:28 +0100 Subject: [PATCH 22/30] Added impulse to delay lsim --- src/delay_systems.jl | 20 ++++++++++++++++ src/types/DelayLtiSystem.jl | 2 +- src/types/PartionedStateSpace.jl | 12 +++++----- test/test_delayed_systems.jl | 41 ++++++++++++++++++++++++++++++++ 4 files changed, 68 insertions(+), 7 deletions(-) diff --git a/src/delay_systems.jl b/src/delay_systems.jl index 64eff4c65..9d89960a3 100644 --- a/src/delay_systems.jl +++ b/src/delay_systems.jl @@ -78,6 +78,7 @@ function dde_param(dx, x, h!, p, t) return end +# TODO Discontinuities in u are not handled well yet. function _lsim(sys::DelayLtiSystem{T}, u!, t::AbstractArray{<:T}, x0::Vector{T}, alg) where T P = sys.P @@ -190,3 +191,22 @@ function Base.step(sys::DelayLtiSystem{T}, t::AbstractVector, kwargs...) where T end return y, tout, x end + + +function impulse(sys::DelayLtiSystem{T}, t::AbstractVector, kwargs...) where T + nu = ninputs(sys) + if t[1] != 0 + throw(ArgumentError("First time point must be 0 in impulse")) + end + u = (out, t) -> (out .= 0) + if nu == 1 + y, tout, x = lsim(sys, u, t, x0=sys.P.B[:,1], kwargs...) + else + x = Array{T}(undef, length(t), nstates(sys), nu) + y = Array{T}(undef, length(t), noutputs(sys), nu) + for i=1:nu + y[:,:,i], tout, x[:,:,i] = lsim(sys[:,i], u, t, x0=sys.P.B[:,i], kwargs...) + end + end + return y, tout, x +end diff --git a/src/types/DelayLtiSystem.jl b/src/types/DelayLtiSystem.jl index aba3880f6..b0ef736a1 100644 --- a/src/types/DelayLtiSystem.jl +++ b/src/types/DelayLtiSystem.jl @@ -18,7 +18,7 @@ function DelayLtiSystem{T}(sys::StateSpace, Tau::Vector{Float64}) where T<:Numbe ny = noutputs(sys) - length(Tau) if nu < 0 || ny < 0 - error("Time vector is too long.") + throw(ArgumentError("Time vector is too long.")) end psys = PartionedStateSpace{StateSpace{T,Matrix{T}}}(sys, nu, ny) diff --git a/src/types/PartionedStateSpace.jl b/src/types/PartionedStateSpace.jl index 67c76b167..e6377193d 100644 --- a/src/types/PartionedStateSpace.jl +++ b/src/types/PartionedStateSpace.jl @@ -161,15 +161,15 @@ function vcat_1(systems::PartionedStateSpace...) A = blockdiag([s.A for s in systems]...) - B1 = vcat([s.B1 for s in systems]...) + B1 = reduce(vcat, [s.B1 for s in systems]) B2 = blockdiag([s.B2 for s in systems]...) C1 = blockdiag([s.C1 for s in systems]...) C2 = blockdiag([s.C2 for s in systems]...) - D11 = vcat([s.D11 for s in systems]...) + D11 = reduce(vcat, [s.D11 for s in systems]) D12 = blockdiag([s.D12 for s in systems]...) - D21 = vcat([s.D21 for s in systems]...) + D21 = reduce(vcat, [s.D21 for s in systems]) D22 = blockdiag([s.D22 for s in systems]...) sysnew = StateSpace(A, [B1 B2], [C1; C2], [D11 D12; D21 D22], Ts) @@ -200,11 +200,11 @@ function hcat_1(systems::PartionedStateSpace...) B1 = blockdiag([s.B1 for s in systems]...) B2 = blockdiag([s.B2 for s in systems]...) - C1 = hcat([s.C1 for s in systems]...) + C1 = reduce(hcat, [s.C1 for s in systems]) C2 = blockdiag([s.C2 for s in systems]...) - D11 = hcat([s.D11 for s in systems]...) - D12 = hcat([s.D12 for s in systems]...) + D11 = reduce(hcat, [s.D11 for s in systems]) + D12 = reduce(hcat, [s.D12 for s in systems]) D21 = blockdiag([s.D21 for s in systems]...) D22 = blockdiag([s.D22 for s in systems]...) diff --git a/test/test_delayed_systems.jl b/test/test_delayed_systems.jl index 735bf1025..0f733f1e7 100644 --- a/test/test_delayed_systems.jl +++ b/test/test_delayed_systems.jl @@ -129,3 +129,44 @@ y2, t2, x2 = step([1.0/s 2/s; 3/s 4/s], t) @test y1 ≈ y2 rtol=1e-15 @test size(x1,1) == length(t) @test size(x1,3) == 2 + + +##################### Test with known solution +K = 2.0 +sys_known = feedback(delay(1)*K/s, 1) + +ystep, t, _ = step(sys_known, 3) + +function y_expected(t, K) + if t < 1 + return 0 + elseif t < 2 + return K*(t-1) + elseif t <= 3 + return K*(t-1)-1/2*K^2*(t-2)^2 + else + throw(ArgumentError("Test not defined here")) + end +end + +@test ystep ≈ y_expected.(t, K) atol = 1e-7 + +function dy_expected(t, K) + if t < 1 + return 0 + elseif t < 2 + return K + elseif t <= 3 + return K - K^2*(t-2) + else + throw(ArgumentError("Test not defined here")) + end +end + +y_impulse, t, _ = impulse(sys_known, 3) + +# TODO Better accuracy for impulse +@test y_impulse ≈ dy_expected.(t, K) rtol=1e-2 +@test maximum(abs, y_impulse - dy_expected.(t, K)) < 1e-2 + +@time [s11; s12] From b842af5501571b46b1deb4ac22e300c15a4b0e83 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Wed, 22 May 2019 09:28:38 +0200 Subject: [PATCH 23/30] clean up rebase --- Project.toml | 2 ++ src/connections.jl | 6 ++---- src/types/StateSpace.jl | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/Project.toml b/Project.toml index ed5cd7481..51342e64d 100644 --- a/Project.toml +++ b/Project.toml @@ -7,6 +7,8 @@ version = "0.5.3" [deps] Colors = "5ae59095-9a9b-59fe-a467-6f913c188581" DSP = "717857b8-e6f2-59f4-9121-6e50c889abd2" +DelayDiffEq = "bcd4f6db-9728-5f36-b5f7-82caef46ccdb" +Interpolations = "a98d9a8b-a2ab-59e6-89dd-64a1c18fca59" IterTools = "c8e1da08-722c-5040-9ed9-7db0dc04731e" LaTeXStrings = "b964fa9f-0449-5b57-a5c2-d3ea65f4040f" LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" diff --git a/src/connections.jl b/src/connections.jl index 5cc918379..f26ab648e 100644 --- a/src/connections.jl +++ b/src/connections.jl @@ -44,8 +44,6 @@ end append(systems::LTISystem...) = append(promote(systems...)...) -function Base.vcat(systems::ST...) where ST <: AbstractStateSpace -# TODO Move size check to wrappers function Base.vcat(systems::DelayLtiSystem...) P = vcat_1([sys.P for sys in systems]...) # See PartitionedStateSpace @@ -60,7 +58,7 @@ function Base.hcat(systems::DelayLtiSystem...) end -function Base.vcat(systems::StateSpace...) +function Base.vcat(systems::ST...) where ST <: AbstractStateSpace # Perform checks nu = systems[1].nu if !all(s.nu == nu for s in systems) @@ -142,7 +140,7 @@ Base.typed_hcat(::Type{T}, X...) where {T<:LTISystem} = hcat(convert.(T, X)...) Base.typed_hcat(::Type{T}, X::Number...) where {T<:LTISystem, N} = hcat(convert.(T, X)...) # Catch special cases where inv(sys) might not be possible after promotion, like improper tf -function /(sys1::Union{StateSpace,DelayLtiSystem}, sys2::LTISystem) +function /(sys1::Union{StateSpace,AbstractStateSpace}, sys2::LTISystem) sys1new, sys2new = promote(sys1, 1/sys2) return sys1new*sys2new end diff --git a/src/types/StateSpace.jl b/src/types/StateSpace.jl index 2b191cf78..bdffa91d4 100644 --- a/src/types/StateSpace.jl +++ b/src/types/StateSpace.jl @@ -284,7 +284,7 @@ Base.:^(sys::AbstractStateSpace, p::Integer) = Base.power_by_squaring(sys, p) ##################################################################### Base.ndims(::AbstractStateSpace) = 2 # NOTE: Also for SISO systems? Base.size(sys::AbstractStateSpace) = (noutputs(sys), ninputs(sys)) # NOTE: or just size(sys.D) -Base.size(sys::AbstractStateSpace, d) = d <= 2 ? size(sys)[d] : 1 +Base.size(sys::AbstractStateSpace, d::Integer) = d <= 2 ? size(sys)[d] : 1 Base.eltype(::Type{S}) where {S<:AbstractStateSpace} = S function Base.getindex(sys::ST, inds...) where ST <: AbstractStateSpace From 834edf6d33305283bc12d0b141c147b953c73d8a Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Wed, 22 May 2019 11:57:26 +0200 Subject: [PATCH 24/30] Update src/delay_systems.jl --- src/delay_systems.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/delay_systems.jl b/src/delay_systems.jl index 9d89960a3..0037686bf 100644 --- a/src/delay_systems.jl +++ b/src/delay_systems.jl @@ -2,7 +2,7 @@ function freqresp(sys::DelayLtiSystem, ω::AbstractVector{T}) where {T <: Real} ny = noutputs(sys) nu = ninputs(sys) - P_fr = ControlSystems.freqresp(sys.P.P, ω); + P_fr = freqresp(sys.P.P, ω); G_fr = zeros(eltype(P_fr), length(ω), ny, nu) From e504b35de79ae341ea6c378ad3b34eb8e8d7608e Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Wed, 22 May 2019 11:57:56 +0200 Subject: [PATCH 25/30] Apply suggestions from code review --- src/delay_systems.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/delay_systems.jl b/src/delay_systems.jl index 0037686bf..790c608ba 100644 --- a/src/delay_systems.jl +++ b/src/delay_systems.jl @@ -83,7 +83,7 @@ function _lsim(sys::DelayLtiSystem{T}, u!, t::AbstractArray{<:T}, x0::Vector{T}, P = sys.P if ~iszero(P.D22) - error("non-zero D22-matrix block is not supported") # Due to limitations in differential equations + throw(ArgumentError("non-zero D22-matrix block is not supported")) # Due to limitations in differential equations end t0 = first(t) From cd0a32ff0291aea00ebafd715395fc0294631c7f Mon Sep 17 00:00:00 2001 From: baggepinnen Date: Wed, 22 May 2019 12:10:34 +0200 Subject: [PATCH 26/30] Replace function wrapper with @nospecialize --- src/delay_systems.jl | 22 ++++++---------------- test/runtests.jl | 6 ++++-- 2 files changed, 10 insertions(+), 18 deletions(-) diff --git a/src/delay_systems.jl b/src/delay_systems.jl index 790c608ba..2aa682f02 100644 --- a/src/delay_systems.jl +++ b/src/delay_systems.jl @@ -21,16 +21,6 @@ function freqresp(sys::DelayLtiSystem, ω::AbstractVector{T}) where {T <: Real} return G_fr end -struct FunctionWrapper <: Function - f::Function -end -(fv::FunctionWrapper)(dx, x, h!, p, t) = fv.f(dx, x, h!, p, t) - -struct UWrapper <: Function - f::Function -end -(fv::UWrapper)(out, t) = fv.f(out, t) - """ `y, t, x = lsim(sys::DelayLtiSystem, t::AbstractArray{<:Real}; u=(out, t) -> (out .= 0), x0=fill(0.0, nstates(sys)), alg=MethodOfSteps(Tsit5()), kwargs...)` @@ -56,11 +46,11 @@ function lsim(sys::DelayLtiSystem{T}, u, t::AbstractArray{<:T}; x0=fill(zero(T), else # If u is a regular u(t) function (out, t) -> (out .= u(t)) end - _lsim(sys, UWrapper(u!), t, x0, alg) + _lsim(sys, u!, t, x0, alg) end function dde_param(dx, x, h!, p, t) - A, B1, B2, C2, D21, Tau, u!, uout, hout, tmp = p[1],p[2],p[3],p[4],p[5],p[6],p[7],p[8],p[9],p[10] + A, B1, B2, C2, D21, Tau, u!, uout, hout, tmp = p u!(uout, t) # uout = u(t) @@ -69,17 +59,17 @@ function dde_param(dx, x, h!, p, t) mul!(tmp, B1, uout) dx .+= tmp - for k=1:length(Tau) # Add each of the delayed signals + @views for k=1:length(Tau) # Add each of the delayed signals u!(uout, t-Tau[k]) # uout = u(t-tau[k]) h!(hout, p, t-Tau[k]) - dk_delayed = dot(view(C2,k,:), hout) + dot(view(D21,k,:), uout) - dx .+= view(B2,:, k) .* dk_delayed + dk_delayed = dot(C2[k,:], hout) + dot(D21[k,:], uout) + dx .+= B2[:, k] .* dk_delayed end return end # TODO Discontinuities in u are not handled well yet. -function _lsim(sys::DelayLtiSystem{T}, u!, t::AbstractArray{<:T}, x0::Vector{T}, alg) where T +function _lsim(sys::DelayLtiSystem{T}, Base.@nospecialize(u!), t::AbstractArray{<:T}, x0::Vector{T}, alg) where T P = sys.P if ~iszero(P.D22) diff --git a/test/runtests.jl b/test/runtests.jl index e1f449fb6..f1ad303c4 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -8,7 +8,9 @@ include("framework.jl") # Local definition to make sure we get warings if we use eye eye_(n) = Matrix{Int64}(I, n, n) -my_tests = ["test_statespace", +my_tests = [ + "test_delayed_systems", + "test_statespace", "test_transferfunction", "test_zpk", "test_promotion", @@ -25,7 +27,7 @@ my_tests = ["test_statespace", "test_lqg", "test_synthesis", "test_partitioned_statespace", - "test_delayed_systems"] + ] # try From 488d8c251f2f73d1d846e6bcac93e13af664b45f Mon Sep 17 00:00:00 2001 From: baggepinnen Date: Wed, 22 May 2019 12:21:09 +0200 Subject: [PATCH 27/30] Use builtin ODE interpolator instead of Interpolations --- Project.toml | 1 - src/ControlSystems.jl | 1 - src/delay_systems.jl | 5 ++--- 3 files changed, 2 insertions(+), 5 deletions(-) diff --git a/Project.toml b/Project.toml index 51342e64d..f17dc19c0 100644 --- a/Project.toml +++ b/Project.toml @@ -8,7 +8,6 @@ version = "0.5.3" Colors = "5ae59095-9a9b-59fe-a467-6f913c188581" DSP = "717857b8-e6f2-59f4-9121-6e50c889abd2" DelayDiffEq = "bcd4f6db-9728-5f36-b5f7-82caef46ccdb" -Interpolations = "a98d9a8b-a2ab-59e6-89dd-64a1c18fca59" IterTools = "c8e1da08-722c-5040-9ed9-7db0dc04731e" LaTeXStrings = "b964fa9f-0449-5b57-a5c2-d3ea65f4040f" LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" diff --git a/src/ControlSystems.jl b/src/ControlSystems.jl index 0c61715f0..94998a0f9 100644 --- a/src/ControlSystems.jl +++ b/src/ControlSystems.jl @@ -87,7 +87,6 @@ export Plots import Base: +, -, *, /, (==), (!=), isapprox, convert, promote_op import Base: getproperty import LinearAlgebra: BlasFloat -import Interpolations export lyap # Make sure LinearAlgebra.lyap is available import Printf, Colors import DSP: conv diff --git a/src/delay_systems.jl b/src/delay_systems.jl index 2aa682f02..d54c974c0 100644 --- a/src/delay_systems.jl +++ b/src/delay_systems.jl @@ -107,16 +107,15 @@ function _lsim(sys::DelayLtiSystem{T}, Base.@nospecialize(u!), t::AbstractArray{ y[:,k] = C1*x[k] + D11*uout #d[:,k] = C2*x[k] + D21*uout end - xitp = Interpolations.interpolate((t,), x, Interpolations.Gridded(Interpolations.Linear())) dtmp = Array{T,1}(undef, size(C2,1)) # Function to evaluate d(t)_i at an arbitrary time - # X is constinuous, so interpoate, u is not + # X is continuous, so interpoate, u is not function dfunc!(tmp::Array{T1}, t, i) where T1 tmp .= if t < t0 T1(0) else - xitp(t) + sol(t) end u!(uout, t) return dot(view(C2,i,:),tmp) + dot(view(D21,i,:),uout) From e9b9d8ca46f6f2810c214b7baf277428e934986e Mon Sep 17 00:00:00 2001 From: baggepinnen Date: Wed, 22 May 2019 13:19:40 +0200 Subject: [PATCH 28/30] add corner case constructor --- src/types/StateSpace.jl | 4 ++++ src/types/TransferFunction.jl | 1 + test/test_delayed_systems.jl | 3 +++ 3 files changed, 8 insertions(+) diff --git a/src/types/StateSpace.jl b/src/types/StateSpace.jl index bdffa91d4..9f2e8d150 100644 --- a/src/types/StateSpace.jl +++ b/src/types/StateSpace.jl @@ -55,6 +55,10 @@ function StateSpace{T,MT}(A::AbstractNumOrArray, B::AbstractNumOrArray, C::Abstr return StateSpace{T,Matrix{T}}(MT(to_matrix(T, A)), MT(to_matrix(T, B)), MT(to_matrix(T, C)), MT(D), Float64(Ts)) end +function StateSpace{T,MT}(sys::StateSpace) where {T, MT <: AbstractMatrix{T}} + StateSpace{T,MT}(sys.A,sys.B,sys.C,sys.D,sys.Ts) +end + function StateSpace(A::AbstractNumOrArray, B::AbstractNumOrArray, C::AbstractNumOrArray, D::AbstractNumOrArray, Ts::Real=0) T = promote_type(eltype(A),eltype(B),eltype(C),eltype(D)) A = to_matrix(T, A) diff --git a/src/types/TransferFunction.jl b/src/types/TransferFunction.jl index 5c91ed141..fbaafa22c 100644 --- a/src/types/TransferFunction.jl +++ b/src/types/TransferFunction.jl @@ -168,6 +168,7 @@ function /(n::Number, G::TransferFunction) end /(G::TransferFunction, n::Number) = G*(1/n) /(G1::TransferFunction, G2::TransferFunction) = G1*(1/G2) +Base.:(/)(sys1::LTISystem, sys2::TransferFunction) = *(promote(sys1, ss(1/sys2))...) # This spcial case is needed to properly handle improper inverse transfer function (1/s) Base.:^(sys::TransferFunction, p::Integer) = Base.power_by_squaring(sys, p) diff --git a/test/test_delayed_systems.jl b/test/test_delayed_systems.jl index 0f733f1e7..d8b04bcd6 100644 --- a/test/test_delayed_systems.jl +++ b/test/test_delayed_systems.jl @@ -1,3 +1,4 @@ +@testset "test_delay_system" begin ω = 0.0:8 # broken: typeof(promote(delay(0.2), ss(1))[1]) == DelayLtiSystem{Float64} @@ -170,3 +171,5 @@ y_impulse, t, _ = impulse(sys_known, 3) @test maximum(abs, y_impulse - dy_expected.(t, K)) < 1e-2 @time [s11; s12] + +end From cff8e693c71841d4bf3ffaaa26047ea09bbab908 Mon Sep 17 00:00:00 2001 From: baggepinnen Date: Wed, 22 May 2019 15:25:45 +0200 Subject: [PATCH 29/30] Forward kwargs to solver --- src/delay_systems.jl | 22 ++++++++++++---------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/src/delay_systems.jl b/src/delay_systems.jl index d54c974c0..3304c87d3 100644 --- a/src/delay_systems.jl +++ b/src/delay_systems.jl @@ -34,10 +34,11 @@ end Can be a constant `Number` or `Vector`, interpreted as `ut .= u` , or Function `ut .= u(t)`, or In-place function `u(ut, t)`. (Slightly more effienct) + `kwargs...`: These are sent to `solve` from DelayDiffEq. Returns: times `t`, and `y` and `x` at those times. """ -function lsim(sys::DelayLtiSystem{T}, u, t::AbstractArray{<:T}; x0=fill(zero(T), nstates(sys)), alg=MethodOfSteps(Tsit5())) where T +function lsim(sys::DelayLtiSystem{T}, u, t::AbstractArray{<:T}; x0=fill(zero(T), nstates(sys)), alg=MethodOfSteps(Tsit5()), kwargs...) where T # Make u! in-place function of u u! = if isa(u, Number) || isa(u,AbstractVector) # Allow for u to be a constant number or vector (uout, t) -> uout .= u @@ -46,7 +47,7 @@ function lsim(sys::DelayLtiSystem{T}, u, t::AbstractArray{<:T}; x0=fill(zero(T), else # If u is a regular u(t) function (out, t) -> (out .= u(t)) end - _lsim(sys, u!, t, x0, alg) + _lsim(sys, u!, t, x0, alg; kwargs...) end function dde_param(dx, x, h!, p, t) @@ -69,7 +70,7 @@ function dde_param(dx, x, h!, p, t) end # TODO Discontinuities in u are not handled well yet. -function _lsim(sys::DelayLtiSystem{T}, Base.@nospecialize(u!), t::AbstractArray{<:T}, x0::Vector{T}, alg) where T +function _lsim(sys::DelayLtiSystem{T}, Base.@nospecialize(u!), t::AbstractArray{<:T}, x0::Vector{T}, alg; kwargs...) where T P = sys.P if ~iszero(P.D22) @@ -95,7 +96,7 @@ function _lsim(sys::DelayLtiSystem{T}, Base.@nospecialize(u!), t::AbstractArray{ p = (A, B1, B2, C2, D21, Tau, u!, uout, hout, tmp) prob = DDEProblem{true}(dde_param, x0, h!, (t[1], t[end]), p, constant_lags=sys.Tau) - sol = DelayDiffEq.solve(prob, alg, saveat=t) + sol = DelayDiffEq.solve(prob, alg; saveat=t, kwargs...) x = sol.u::Array{Array{T,1},1} # the states are labeled u in DelayDiffEq @@ -162,7 +163,7 @@ end iscontinuous(sys::DelayLtiSystem) = true -function Base.step(sys::DelayLtiSystem{T}, t::AbstractVector, kwargs...) where T +function Base.step(sys::DelayLtiSystem{T}, t::AbstractVector; kwargs...) where T nu = ninputs(sys) if t[1] != 0 throw(ArgumentError("First time point must be 0 in step")) @@ -170,31 +171,32 @@ function Base.step(sys::DelayLtiSystem{T}, t::AbstractVector, kwargs...) where T u = (out, t) -> (t < 0 ? out .= 0 : out .= 1) x0=fill(zero(T), nstates(sys)) if nu == 1 - y, tout, x = lsim(sys, u, t, x0=x0, kwargs...) + y, tout, x = lsim(sys, u, t; x0=x0, kwargs...) else x = Array{T}(undef, length(t), nstates(sys), nu) y = Array{T}(undef, length(t), noutputs(sys), nu) for i=1:nu - y[:,:,i], tout, x[:,:,i] = lsim(sys[:,i], u, t, x0=x0, kwargs...) + y[:,:,i], tout, x[:,:,i] = lsim(sys[:,i], u, t; x0=x0, kwargs...) end end return y, tout, x end -function impulse(sys::DelayLtiSystem{T}, t::AbstractVector, kwargs...) where T +function impulse(sys::DelayLtiSystem{T}, t::AbstractVector; kwargs...) where T nu = ninputs(sys) + iszero(sys.P.D12) || @warn("Impulse with a direct term from input to delay vector leads to poor accuracy.") if t[1] != 0 throw(ArgumentError("First time point must be 0 in impulse")) end u = (out, t) -> (out .= 0) if nu == 1 - y, tout, x = lsim(sys, u, t, x0=sys.P.B[:,1], kwargs...) + y, tout, x = lsim(sys, u, t; x0=sys.P.B[:,1], kwargs...) else x = Array{T}(undef, length(t), nstates(sys), nu) y = Array{T}(undef, length(t), noutputs(sys), nu) for i=1:nu - y[:,:,i], tout, x[:,:,i] = lsim(sys[:,i], u, t, x0=sys.P.B[:,i], kwargs...) + y[:,:,i], tout, x[:,:,i] = lsim(sys[:,i], u, t; x0=sys.P.B[:,i], kwargs...) end end return y, tout, x From a3a9e6c4f5e2666e4cfc3d3aba1f55566f3226e6 Mon Sep 17 00:00:00 2001 From: baggepinnen Date: Wed, 22 May 2019 15:54:08 +0200 Subject: [PATCH 30/30] increase tol and clean code --- src/delay_systems.jl | 6 +++--- test/test_delayed_systems.jl | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/delay_systems.jl b/src/delay_systems.jl index 3304c87d3..e362102ba 100644 --- a/src/delay_systems.jl +++ b/src/delay_systems.jl @@ -116,7 +116,7 @@ function _lsim(sys::DelayLtiSystem{T}, Base.@nospecialize(u!), t::AbstractArray{ tmp .= if t < t0 T1(0) else - sol(t) + sol(t) # solution object has built in interpolator of same order as solver. end u!(uout, t) return dot(view(C2,i,:),tmp) + dot(view(D21,i,:),uout) @@ -130,7 +130,7 @@ function _lsim(sys::DelayLtiSystem{T}, Base.@nospecialize(u!), t::AbstractArray{ end end - return y', t, hcat(x...)' + return y', t, reduce(hcat ,x)' end @@ -138,7 +138,7 @@ end function _bounds_and_features(sys::DelayLtiSystem, plot::Symbol) ws, pz = _bounds_and_features(sys.P.P, plot) logtau = log10.(abs.(sys.Tau)) - logtau = logtau[logtau .> -4] # Ignore low frequency + logtau = filter(x->x>4, logtau) # Ignore low frequency if isempty(logtau) return ws, pz end diff --git a/test/test_delayed_systems.jl b/test/test_delayed_systems.jl index d8b04bcd6..286387317 100644 --- a/test/test_delayed_systems.jl +++ b/test/test_delayed_systems.jl @@ -150,7 +150,7 @@ function y_expected(t, K) end end -@test ystep ≈ y_expected.(t, K) atol = 1e-7 +@test ystep ≈ y_expected.(t, K) atol = 1e-13 function dy_expected(t, K) if t < 1