# Linearizing ModelingToolkit ODESystem

I am having my bi-monthly go at ModelingToolkit and have reached a point where I can create an ODESystem from ControlSystems.jl StateSpace and connect a few of those togeher and simulate, nice As a next step, I would like to go the other way, given an ODESystem, I would like to obtain an LTI system on the form

\dot x = Ax + Bu\\ y = Cx + Du

where x are the state variables, u are inputs and y are outputs.

The function calculate_jacobian appears to do something reasonable towards obtaining the A matrix, and Symbolics.jacobian also seems useful. My main problem appears to be to handle inputs and outputs. The matrices B,C and D require me to linearize the state and output equations w.r.t. the input and states. So far, I have reached to the function below. It works for the most primitive of systems, but quickly fails when I compose a few systems.

function ControlSystems.ss(sys::ODESystem, inputs, outputs)
inputs, outputs = Set.((inputs, outputs))
x       = ModelingToolkit.get_states(sys) # will contain x, and sometimes u, y
o       = ModelingToolkit.get_observed(sys) # equations
x       = [x; getproperty.(o, :lhs)]
u       = filter(s->s ∈ inputs, x)  # try to "extract" inputs
y       = filter(s->s ∈ outputs, x) # try to "extract" outputs
x       = setdiff(setdiff(x, u), y) # remove states that are not inputs or outputs
eqs     = sys.eqs
diffeqs = [e.rhs for e in eqs if Symbolics.is_derivative(e.lhs)] # state equations
yeqs    = [e.rhs for e in eqs if e.lhs ∈ outputs]                # output equations
A       = Symbolics.jacobian(diffeqs, x)
B       = Symbolics.jacobian(diffeqs, u)
C       = Symbolics.jacobian(yeqs, x)
D       = Symbolics.jacobian(yeqs, u)
ss(A,B,C,D)
end


Has anyone cooked up something similar and feel like sharing? My full code example is given below

code
using ModelingToolkit, ControlSystems
using ControlSystems: ssdata, AbstractStateSpace, Continuous, nstates, noutputs, ninputs
import ModelingToolkit: ODESystem
using DifferentialEquations
using ModelingToolkit.Symbolics

ModelingToolkit.ODESystem(sys::LTISystem; kwargs...) = ODESystem(ss(sys); kwargs...)

"Create an ODESystem from ControlSystems.StateSpace"
function ModelingToolkit.ODESystem(sys::AbstractStateSpace{Continuous};
name::Symbol,
x0 = zeros(sys.nx),
x_names = [Symbol("x$i") for i in 1:sys.nx], u_names = sys.nu == 1 ? [:u] : [Symbol("u$i") for i in 1:sys.nu],
y_names = sys.ny == 1 ? [:y] : [Symbol("y$i") for i in 1:sys.ny], ) A,B,C,D = ssdata(sys) nx,ny,nu = sys.nx, sys.ny, sys.nu # ny == nu == 1 || @warn("MIMO systems are poorly supported for now https://github.com/SciML/ModelingToolkit.jl/issues/605") @parameters t x = [Num(Variable{ModelingToolkit.FnType{Tuple{Any},Real}}(name))(t) for name in x_names] u = [Num(Variable{ModelingToolkit.FnType{Tuple{Any},Real}}(name))(t) for name in u_names] y = [Num(Variable{ModelingToolkit.FnType{Tuple{Any},Real}}(name))(t) for name in y_names] Dₜ = Differential(t) eqs = [ Dₜ.(x) .~ A*x .+ B*u y .~ C*x .+ D*u ] ODESystem(eqs; name, defaults = Dict(x .=> x0)) end "connect input to ODESystem" function connect(input, sys::ODESystem; name=Symbol("$(sys.name) with input"))
@parameters t
@variables u(t) y(t)
ODESystem([
u ~ input
sys.u ~ input
y ~ sys.y
], t; systems=[sys], name)
end

function connect(input::Function, sys::ODESystem; name=Symbol("$(sys.name) with input")) @parameters t @variables u(t) y(t) ODESystem([ sys.u ~ input(u) y ~ sys.y ], t; systems=[sys], name) end "connect output of one sys to input of other" function connect(sys1::ODESystem, sys2::ODESystem; name=Symbol("$(sys1.name)*$(sys2.name)")) @parameters t @variables u(t) y(t) ODESystem([ u ~ sys1.u sys1.y ~ sys2.u y ~ sys2.y ], t; systems=[sys1, sys2], name) end "form feedback interconnection, i.e., input is r-y" function ControlSystems.feedback(loopgain::ODESystem, ref; name=Symbol("feedback$(loopgain.name)"))
@parameters t
@variables u(t) y(t)
ODESystem([
u ~ ref
ref - loopgain.y ~ loopgain.u
y ~ loopgain.y
], t; systems=[loopgain], name)
end

function ControlSystems.feedback(loopgain::ODESystem, ref; name=Symbol("feedback $(loopgain.name)")) @parameters t @variables u(t) y(t) # @variables y(t) ODESystem([ u ~ ref ref - loopgain.y ~ loopgain.u y ~ loopgain.y ], t; systems=[loopgain], name) end function ControlSystems.feedback(loopgain::ODESystem, ref::ODESystem; name=Symbol("feedback$(loopgain.name)*\$(ref.name)"))
@parameters t
@variables u(t) y(t)
ODESystem([
u ~ ref.u
ref.y - loopgain.y ~ loopgain.u
y ~ loopgain.y
], t; systems=[loopgain, ref], name)
end

numeric(x::Num) = x.val

# This approach is very brittle
function ControlSystems.ss(sys::ODESystem, inputs, outputs)
inputs, outputs = Set.((inputs, outputs))
x       = ModelingToolkit.get_states(sys) # will contain x, and sometimes u, y
o       = ModelingToolkit.get_observed(sys) # equations
x       = [x; getproperty.(o, :lhs)]
u       = filter(s->s ∈ inputs, x)
y       = filter(s->s ∈ outputs, x)
x       = setdiff(setdiff(x, u), y) # remove states that are not inputs or outputs
eqs     = sys.eqs
diffeqs = [e.rhs for e in eqs if Symbolics.is_derivative(e.lhs)]
aeqs    = [e.rhs for e in eqs if e.lhs ∈ outputs]
@show A       = Symbolics.jacobian(diffeqs, x) .|> numeric
@show B       = Symbolics.jacobian(diffeqs, u) .|> numeric
@show C       = Symbolics.jacobian(aeqs, x)    .|> numeric
@show D       = Symbolics.jacobian(aeqs, u)    .|> numeric
ss(A,B,C,D)
end

## Test SISO (single input, single output) system
@parameters t

P0 = tf(1.0, [1, 1])                         |> ss
C0 = pid(kp = 1, ki = 1) * tf(1, [0.01, 1])  |> ss

@named P           = ODESystem(P0)
# @named nonlinear_P = connect(x->sign(x)*sqrt(abs(x)), P) # apply input-nonlinearity
@named C           = ODESystem(C0)
@named loopgain    = connect(C, P)
@named fb          = feedback(loopgain, sin(t))
@show fb = structural_simplify(fb)

x0 = Pair[
# fb.loopgain.nonlinear_P.P.x1 => 1 # a bit inconvenient to specify initial states
# loopgain.nonlinear_P.P.x1 => 1
loopgain.P.x1 => 1
# P.x1 => 1
]
p = Pair[]

prob = ODEProblem(simplified_sys, x0, (0.0, 10.0), p)
sol = solve(prob, Tsit5())
plot(sol)

# === Go the other way, ODESystem -> StateSpace ================================
using Test
x = ModelingToolkit.get_states(P) # I haven't figured out a good way to access states, so this is a bit manual and ugly
P02 = ss(P, x[2:2], x[3:3])
@test P02 == P0 # verify that we get back what we started with

# same for controller
x = ModelingToolkit.get_states(C)
C02 = ss(C, x[3:3], x[4:4])
@test C02 == C0

# Now try do the same with the feedback interconnection. This fails, I cannot figure out how to provide the input I want to linearize w.r.t. to. Below is an attempt by creating an input variable r, but that causes structural_simplify to complain.
@variables r(t)
@named fbu = feedback(loopgain, r)
@show fbu = structural_simplify(fbu)
fbus = ModelingToolkit.get_states(fbu)
fb2 = @nonamespace ss(fbu, [r], [fbu.y])
feedback(P0*C0) # fb2 should be similar to this feeback interconnection calculated by ControlSystems

5 Likes

This is useful. On a longer horizon… would it be of interest to generalize the ideas for cases such as DAEs, and PDAEs? E.g., for DAEs formulate a model of form:

E\frac{dx}{dt} = Ax+Bu \\ y = Cx+Du

What would be a useful linear form of PDEs/PDAEs?

Instead of using the local accessors, why not use the flattened accessors, i.e. states(sys)? That will generate the flattened linearized system.

function ControlSystems.ss(sys::ODESystem, inputs, outputs)
inputs, outputs = Set.((inputs, outputs))
x       = ModelingToolkit.states(sys) # will contain x, and sometimes u, y
o       = ModelingToolkit.observed(sys) # equations
x       = [x; getproperty.(o, :lhs)]
u       = filter(s->s ∈ inputs, x)  # try to "extract" inputs
y       = filter(s->s ∈ outputs, x) # try to "extract" outputs
x       = setdiff(setdiff(x, u), y) # remove states that are not inputs or outputs
eqs     = equations(sys)
diffeqs = [e.rhs for e in eqs if Symbolics.is_derivative(e.lhs)] # state equations
yeqs    = [e.rhs for e in eqs if e.lhs ∈ outputs]                # output equations
A       = Symbolics.jacobian(diffeqs, x)
B       = Symbolics.jacobian(diffeqs, u)
C       = Symbolics.jacobian(yeqs, x)
D       = Symbolics.jacobian(yeqs, u)
ss(A,B,C,D)
end


might be what you’re looking for.