Linearizing ModelingToolkit ODESystem

[baggepinnen]

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

[BLI]

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?

[ChrisRackauckas]

@baggepinnen

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.

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.

[baggepinnen]

Linearization of dynamical systems with control inputs is now provided by MTK. The docs have not yet caught up to the developements, but the test file has some nice examples and the docstrings for linearize are in place.