New calculate_statespace function for AbstractODESystem

src/ModelingToolkit.jl

@@ -167,6 +167,7 @@ export DiscreteSystem, DiscreteProblem
 
 export calculate_jacobian, generate_jacobian, generate_function
 export calculate_control_jacobian, generate_control_jacobian
+export calculate_statespace
 export calculate_tgrad, generate_tgrad
 export calculate_gradient, generate_gradient
 export calculate_factorized_W, generate_factorized_W

src/systems/diffeqs/abstractodesystem.jl

@@ -727,3 +727,27 @@ end
 
 isdifferential(expr) = istree(expr) && operation(expr) isa Differential
 isdiffeq(eq) = isdifferential(eq.lhs)
+
+"""
+    calculate_statespace(sys::AbstractODESystem, subs::Dict = sys.defaults; matrix_eltype::Type{T} = Float64) where T
+
+Returns a tuple of the linearized `A` and `B` matrices for a state space model. 
+"""
+function calculate_statespace(sys::AbstractODESystem, subs::Dict = defaults(sys); matrix_eltype::Type{T} = Float64) where T
+
+    J = calculate_jacobian(sys)
+    C = calculate_control_jacobian(sys)
+
+    A = Matrix{T}(undef, size(J))
+    B = Matrix{T}(undef, size(C))
+
+    for I in CartesianIndices(A)
+        A[I] = value(substitute(J[I], subs))
+    end
+
+    for I in CartesianIndices(B)
+        B[I] = value(substitute(C[I], subs))
+    end
+
+    return A,B
+end

test/odesystem.jl

@@ -379,4 +379,21 @@ let
         reshape(Num[0,1], 2, 1)
     )
 
-end
+end
+
+# check statespace
+let 
+    @parameters t f k d
+    @variables x(t) ẋ(t)
+    δ = Differential(t)
+
+    eqs = [δ(x) ~ ẋ, δ(ẋ) ~ f - k*x - d*ẋ]
+    sys = ODESystem(eqs, t, [x, ẋ], [f, d, k]; controls = [f])
+
+    A = reshape(Float64[0, -1, 1, -2], 2, 2)
+    B = reshape(Float64[0, 1], 2, 1)
+    @test isequal(
+        calculate_statespace(sys, Dict([k => 1, d => 2]); matrix_eltype=Float64),
+        (A, B)
+    )
+end