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 :slight_smile:

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.