add solver for trajectories w/o bloating

src/Algorithms/BOX/BOX.jl

@@ -68,14 +68,19 @@ end
 step_size(alg::BOX) = alg.δ
 numtype(::BOX{N}) where {N} = N
 
-function rsetrep(::BOX{N, AM, Val{false}, D, R}) where {N, AM, D, R}
+function setrep(::BOX{N, AM, Val{false}, D, R}) where {N, AM, D, R}
     VT = Vector{N}
-    RT = ReachSet{N, Hyperrectangle{N, VT, VT}}
+    ST = Hyperrectangle{N, VT, VT}
 end
 
 function rsetrep(::BOX{N, AM, Val{true}, Val{n}, R}) where {N, AM, n, R}
     VT = SVector{n, N}
-    RT = ReachSet{N, Hyperrectangle{N, VT, VT}}
+    ST = Hyperrectangle{N, VT, VT}
+end
+
+function rsetrep(alg::BOX{N}) where {N}
+    ST = setrep(alg)
+    RT = ReachSet{N, ST}
 end
 
 include("post.jl")

src/Algorithms/ORBIT/ORBIT.jl

@@ -0,0 +1,24 @@
+struct ORBIT{N, VT, AM} <: AbstractContinuousPost
+    δ::N
+    approx_model::AM
+end
+
+step_size(alg::ORBIT) = alg.δ
+numtype(::ORBIT{N}) where {N} = N
+
+# convenience constructor using symbols
+function ORBIT(; δ::N, approx_model::AM=NoBloating(exp=:base, setops=:concrete)) where {N, AM}
+    VT = Vector{N}
+    return ORBIT{N, VT, AM}(δ, approx_model)
+end
+
+function setrep(::ORBIT{N, VT, AM}) where {N, VT, AM}
+    Singleton{N, VT}
+end
+
+function rsetrep(alg::ORBIT{N}) where {N}
+    ST = sertrep(alg)
+    ReachSet{N, ST}
+end
+
+include("post.jl")

src/Algorithms/ORBIT/post.jl

@@ -0,0 +1,98 @@
+using LazySets.Arrays: _vector_type
+
+function post(alg::ORBIT{N, VT, AM}, ivp::IVP{<:AbstractContinuousSystem}, tspan;
+              Δt0::TimeInterval=zeroI, kwargs...) where {N, VT, AM}
+
+    @unpack δ, approx_model = alg
+
+    if haskey(kwargs, :NSTEPS)
+        NSTEPS = kwargs[:NSTEPS]
+        T = NSTEPS * δ
+    else
+        # get time horizon from the time span imposing that it is of the form (0, T)
+        T = _get_T(tspan, check_zero=true, check_positive=true)
+        NSTEPS = ceil(Int, T / δ)
+    end
+
+    # normalize system to canonical form
+    ivp_norm = _normalize(ivp)
+
+    # discretize system
+    ivp_discr = discretize(ivp_norm, δ, approx_model)
+    Φ = state_matrix(ivp_discr)
+    Ω0 = initial_state(ivp_discr)
+
+    X = stateset(ivp_discr)
+    V = inputset(ivp_discr)
+
+    # preallocate output flowpipe
+    ST = Singleton{N, VT}
+    F = Vector{ReachSet{N, ST}}(undef, NSTEPS)
+
+    _orbit!(F, Φ, Ω0, V, NSTEPS, δ, X, Δt0)
+
+    return Flowpipe(F)
+end
+
+# Given a matrix Φ and vectors Ω0 and V, compute the sequence:
+#
+# Ω0
+# Φ*Ω0 + V
+# Φ^2*Ω0 + Φ*V + V
+# ....
+# Φ^(NSTEPS-1)*Ω0  + Φ^(NSTEPS-2)*V + ... + Φ*V + V
+function _orbit!(F, Φ::AbstractMatrix{N}, Ω0, V, NSTEPS, δ, X::Universe, Δt0) where {N}
+    # preallocate output sequence
+    n = size(Φ, 1)
+    VT = _vector_type(typeof(Φ))
+    out = Vector{VT}(undef, NSTEPS)
+    @inbounds for i in 1:NSTEPS
+        out[i] = VT(undef, n)
+    end
+
+    # compute output sequence
+    _orbit!(out, Φ, Ω0, V, NSTEPS)
+
+    # fill singleton reach-set sequence
+    Δt = (zero(N) .. δ) + Δt0
+    for k in 1:NSTEPS
+        xk = Singleton(out[k])
+        F[k] = ReachSet(xk, Δt)
+        Δt += δ
+    end
+    return out
+end
+
+# case with zero homogeneous solution Ω0 = 0
+function _orbit!(out, Φ::AbstractMatrix{N}, Ω0::ZeroSet, V::Singleton, NSTEPS) where {N}
+    v = element(V)
+    out[1] = zeros(N, n)
+    copy!(out[2], v)
+    @inbounds for i in 2:NSTEPS-1
+        mul!(out[i+1], Φ, out[i])
+        out[i+1] .+= v
+    end
+    return out
+end
+
+# case without input V = 0
+function _orbit!(out, Φ::AbstractMatrix{N}, Ω0::Singleton, V::Union{ZeroSet, Nothing}, NSTEPS) where {N}
+    x = element(Ω0)
+    copy!(out[1], x)
+    @inbounds for i in 1:NSTEPS-1
+        mul!(out[i+1], Φ, out[i])
+    end
+    return out
+end
+
+# general case
+function _orbit!(out, Φ::AbstractMatrix{N}, Ω0::Singleton, V::Singleton, NSTEPS) where {N}
+    v = element(V)
+    x = element(Ω0)
+    copy!(out[1], x)
+    @inbounds for i in 1:NSTEPS-1
+        mul!(out[i+1], Φ, out[i])
+        out[i+1] .+= v
+    end
+    return out
+end

src/Continuous/discretization.jl

@@ -274,8 +274,7 @@ function discretize(ivp::IVP{<:CLCS, <:LazySet}, δ, alg::NoBloating)
         Φ = _exp(A, δ, alg.exp)
     end
 
-    Ω0 = copy(X0)
-    Ω0 = _apply_setops(Ω0, alg.setops)
+    Ω0 = copy(X0) # setops don't apply
     X = stateset(ivp)
     Sdiscr = ConstrainedLinearDiscreteSystem(Φ, X)
     return InitialValueProblem(Sdiscr, Ω0)
@@ -285,16 +284,30 @@ end
 function discretize(ivp::IVP{<:CLCCS, <:LazySet}, δ, alg::NoBloating)
     A = state_matrix(ivp)
     X0 = initial_state(ivp)
+    U = next_set(inputset(ivp), 1)
 
     Φ = _exp(A, δ, alg.exp)
-    M = Φ₁(A, δ, alg.exp)
-    U = next_set(inputset(ivp), 1) # inputset(ivp)
-    Ud = M * U # TODO assert alg.setops  / alg.inputsetops are lazy
-    In = IdentityMultiple(one(eltype(A)), size(A, 1))
+    Ω0, V = _discretize_nobloating(A, X0, U, δ, alg)
 
-    Ω0 = copy(X0)
-    Ω0 = _apply_setops(Ω0, alg.setops)
+    In = IdentityMultiple(one(eltype(A)), size(A, 1))
     X = stateset(ivp)
-    Sdiscr = ConstrainedLinearControlDiscreteSystem(Φ, In, X, Ud)
+    Sdiscr = ConstrainedLinearControlDiscreteSystem(Φ, In, X, V)
     return InitialValueProblem(Sdiscr, Ω0)
 end
+
+function _discretize_nobloating(A, X0, U, δ, alg::NoBloating{EM, Val{:lazy}}) where {EM}
+    M = Φ₁(A, δ, alg.exp)
+    V = M * U
+    Ω0 = _initial_state(X0)
+    return Ω0, V
+end
+
+function _discretize_nobloating(A, X0, U, δ, alg::NoBloating{EM, Val{:concrete}}) where {EM}
+    M = Φ₁(A, δ, alg.exp)
+    V = linear_map(M, U)
+    Ω0 = _initial_state(X0)
+    return Ω0, V
+end
+
+_initial_state(X0::CartesianProduct{N, <:Singleton{N}, <:Singleton{N}}) where {N} = convert(Singleton, X0)
+_initial_state(X0) = X0

src/Continuous/exponentiation.jl

@@ -146,6 +146,35 @@ end
 @inline _Φ₁(P, n, ::Val{:lazy}) = sparse(get_columns(P, (n+1):2*n)[1:n, :])
 @inline _Φ₁(P, n, ::Val{:pade}) = P[1:n, (n+1):2*n]
 
+# compute Φ₁(A, δ)U = A^{-1}(exp(Aδ) - I) U, where U is a vector
+# assumes that A is invertible
+function Φ₁(A::AbstractMatrix{N}, δ, ::Val{:inverse}) where {N}
+    Φ = exp(Matrix(A) * δ) #  TODO cache
+    n = size(A, 1)
+    In = Matrix(one(N)*I, n, n)
+    Ainv = inv(A)
+    return Ainv * (Φ - In)
+end
+
+# compute Φ₁(A, δ)U = A^{-1}(exp(Aδ) - I) U, where U is a vector
+# assumes that A is invertible
+function Φ₁(A::AbstractMatrix, δ, U::AbstractVector, ::Val{:inverse})
+    Φ = exp(Matrix(A) * δ) #  TODO cache
+    x = Φ * U - U
+    return A \ x
+end
+
+function load_Φ₁_krylov()
+return quote
+
+function Φ₁(A::AbstractMatrix, δ, U::AbstractVector, ::Val{:krylov}; m=30, tol=1e-10)
+    w = expv(1.0 * δ, A, U, m=m, tol=tol)
+    x = w - U
+    return A \ x
+end
+
+end end  # quote / load_Φ₁_krylov()
+
 """
     Φ₂(A::AbstractMatrix, δ, method)
 
@@ -199,13 +228,18 @@ end
 @inline _Φ₂(P, n, ::Val{:lazy}) = sparse(get_columns(P, (2*n+1):3*n)[1:n, :])
 @inline _Φ₂(P, n, ::Val{:pade}) = P[1:n, (2*n+1):3*n]
 
-#=
-TODO define and use inverse method
-if method == :inverse
-    return _Φ₂_inverse(A, δ)
+# Φ₂ = A^{-2} (exp(A*δ) - I - A*δ)
+function Φ₂(A::AbstractMatrix{N}, δ, ::Val{:inverse}) where {N}
+    Φ = exp(Matrix(A) * δ)
+    n = size(A, 1)
+    In = Matrix(1.0I, n, n)
+    B = Φ - In - Aδ
+    Ainv = inv(A)
+    Ainvsqr = Ainv^2
+    return Ainvsqr * B
 end
-=#
 
+# TODO cleanup
 @inline function _Φ₂_inverse(A::IdentityMultiple, δ::Float64)
     λ = A.M.λ
     @assert !iszero(λ) "the given identity multiple is not invertible"
@@ -214,6 +248,7 @@ end
     return IdentityMultiple(α, size(A, 1))
 end
 
+# TODO cleanup
 @inline function _Φ₂_inverse(A::AbstractMatrix, δ::Float64)
     @assert isinvertible(A) "the given matrix should be invertible"
     Ainv = inv(A)

src/Continuous/normalization.jl

@@ -29,6 +29,7 @@ const SOLCS = SecondOrderLinearContinuousSystem
 const SOACS = SecondOrderAffineContinuousSystem
 const SOCLCCS = SecondOrderConstrainedLinearControlContinuousSystem
 const SOCACCS = SecondOrderConstrainedAffineControlContinuousSystem
+const SecondOrderSystem = Union{SOLCS, SOACS, SOCLCCS, SOCACCS}
 
 # continuous systems that are handled by this library
 iscontinuoussystem(T::Type{<:AbstractSystem}) = false
@@ -568,37 +569,29 @@ function _normalize(ivp::IVP{<:AbstractContinuousSystem}, ::Type{AbstractNonline
     throw(ArgumentError("can't normalize this nonlinear initial-value problemof type $(typeof(ivp))"))
 end
 
-
+_normalize_initial_state(ivp, X0) = X0
+_normalize_initial_state(ivp, X0::AbstractVector) = Singleton(X0)
+_normalize_initial_state(ivp, X0::IA.Interval) = convert(Interval, X0)
+_normalize_initial_state(ivp, X0::IA.IntervalBox) = convert(Hyperrectangle, X0)
+_normalize_initial_state(ivp, X0::Number) = Singleton([X0])
+_normalize_initial_state(ivp::IVP{<:SecondOrderSystem}, X0::Tuple{<:AbstractVector, <:AbstractVector}) = Singleton(vcat(X0[1], X0[2]))
 
 const CanonicalLinearContinuousSystem = Union{CLCS, CLCCS}
 
 function _normalize(ivp::IVP{<:AbstractContinuousSystem}, ::Type{AbstractLinearContinuousSystem})
 
     # initial states normalization
     X0 = initial_state(ivp)
-    if X0 isa AbstractVector
-        X0_norm = Singleton(X0)
-    elseif X0 isa IA.Interval
-        X0_norm = convert(Interval, X0)
-    elseif X0 isa IA.IntervalBox
-        X0_norm = convert(Hyperrectangle, X0)
-    elseif X0 isa Number
-        X0_norm = Singleton([X0])
-    else
-        X0_norm = X0
-    end
+    X0_norm = _normalize_initial_state(ivp, X0)
 
     # system's normalization
     S = system(ivp)
     S_norm = normalize(S)
 
-    #=
     if S_norm === S && X0_norm === X0
         ivp_norm = ivp
     else
         ivp_norm = IVP(S_norm, X0_norm)
     end
-    =#
-    ivp_norm = IVP(S_norm, X0_norm)
     return ivp_norm
 end

src/Continuous/solve.jl

@@ -168,11 +168,13 @@ function _check_dim(S, X0; throw_error::Bool=true)
     return false
 end
 
+_dim(X0::Tuple{VT, VT}) where {N, VT<:AbstractVector{N}} = length(X0[1]) + length(X0[2])
+
 function _check_dim(S::Union{SecondOrderLinearContinuousSystem,
                              SecondOrderAffineContinuousSystem,
                              SecondOrderConstrainedLinearControlContinuousSystem,
                              SecondOrderConstrainedAffineControlContinuousSystem},
-                             X0; throw_error::Bool=true)
+                    X0; throw_error::Bool=true)
     n = statedim(S)
     d = _dim(X0)
     d == 2*n && return true

src/Flowpipes/setops.jl

@@ -99,6 +99,13 @@ function Base.convert(::Type{Hyperrectangle{N, Vector{N}, Vector{N}}},
     return Hyperrectangle(Vector(H.center), Vector(H.radius))
 end
 
+function Base.convert(::Type{Singleton},
+                      cp::CartesianProduct{N, S1, S2}) where{N, S1<:Singleton{N}, S2<:Singleton{N}}
+    x = element(cp.X)
+    y = element(cp.Y)
+    return Singleton(vcat(x, y))
+end
+
 # =========================
 # In-place ops
 # =========================

src/Initialization/exports.jl

@@ -12,8 +12,9 @@ export
     GLGM06,
     INT,
     LGG09,
-    TMJets,
+    ORBIT,
     QINT,
+    TMJets,
 
 # Approximation models
     Forward,

src/Initialization/init_ExponentialUtilities.jl

@@ -2,5 +2,6 @@ eval(quote
     using .ExponentialUtilities: expv, expv!, arnoldi, arnoldi!, KrylovSubspace
 end)
 
+eval(load_Φ₁_krylov())
 eval(load_krylov_LGG09_homog())
 eval(load_krylov_LGG09_inhomog())

src/ReachabilityAnalysis.jl

@@ -40,6 +40,7 @@ include("Algorithms/BFFPSV18/BFFPSV18.jl")
 include("Algorithms/ASB07/ASB07.jl")
 #include("Algorithms/ASB07d/ASB07d.jl")
 #include("Algorithms/A17/A17.jl")
+include("Algorithms/ORBIT/ORBIT.jl")
 
 # Nonlinear
 include("Algorithms/TMJets/TMJets.jl")