add option to simplify causality in a different way (#73)

* add option to simplify causality in a different way

* provide some variables for operating points

* fixes in batchlin

* init fixes

* rm import

* more inits

Author: baggepinnen

docs/Project.toml

@@ -6,7 +6,8 @@ Documenter = "e30172f5-a6a5-5a46-863b-614d45cd2de4"
 ModelingToolkit = "961ee093-0014-501f-94e3-6117800e7a78"
 ModelingToolkitStandardLibrary = "16a59e39-deab-5bd0-87e4-056b12336739"
 MonteCarloMeasurements = "0987c9cc-fe09-11e8-30f0-b96dd679fdca"
-OrdinaryDiffEq = "1dea7af3-3e70-54e6-95c3-0bf5283fa5ed"
+OrdinaryDiffEqNonlinearSolve = "127b3ac7-2247-4354-8eb6-78cf4e7c58e8"
+OrdinaryDiffEqRosenbrock = "43230ef6-c299-4910-a778-202eb28ce4ce"
 Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80"
 RobustAndOptimalControl = "21fd56a4-db03-40ee-82ee-a87907bee541"
 SymbolicControlSystems = "886cb795-8fd3-4b11-92f6-8071e46f71c5"

docs/src/batch_linearization.md

@@ -8,7 +8,6 @@ This example will demonstrate how to linearize a nonlinear ModelingToolkit model
 The model will be a simple Duffing oscillator:
 ```@example BATCHLIN
 using ControlSystemsMTK, ModelingToolkit, MonteCarloMeasurements, ModelingToolkitStandardLibrary.Blocks
-using ModelingToolkit: getdefault
 unsafe_comparisons(true)
 
 # Create a model
@@ -26,15 +25,15 @@ eqs = [D(x) ~ v
        y.u ~ x]
 
 
-@named duffing = ODESystem(eqs, t, systems=[y, u], defaults=[u.u => 0])
+@named duffing = ODESystem(eqs, t, systems=[y, u])
 ```
 
 ## Batch linearization
 To perform batch linearization, we create a vector of operating points, and then linearize the model around each of these points. The function [`batch_ss`](@ref) does this for us, and returns a vector of `StateSpace` models, one for each operating point. An operating point is a `Dict` that maps variables in the MTK model to numerical values. In the example below, we simply sample the variables uniformly within their bounds specified when we created the variables (normally, we might want to linearize on stationary points)
 ```@example BATCHLIN
 N = 16 # Number of samples
 xs = range(getbounds(x)[1], getbounds(x)[2], length=N)
-ops = Dict.(x .=> xs)
+ops = Dict.(u.u => 0, x .=> xs)
 ```
 
 Just like [`ModelingToolkit.linearize`](@ref), [`batch_ss`](@ref) takes the set of inputs and the set of outputs to linearize between.
@@ -96,7 +95,7 @@ If you plot the Nyquist curve using the `plotly()` backend rather than the defau
 Above, we tuned one controller for each operating point, wouldn't it be nice if we had some features to simulate a [gain-scheduled controller](https://en.wikipedia.org/wiki/Gain_scheduling) that interpolates between the different controllers depending on the operating pont? [`GainScheduledStateSpace`](@ref) is such a thing, we show how to use it below. For fun, we simulate some reference step responses for each individual controller in the array `Cs` and end with simulating the gain-scheduled controller.
 
 ```@example BATCHLIN
-using OrdinaryDiffEq
+using OrdinaryDiffEqNonlinearSolve, OrdinaryDiffEqRosenbrock
 using DataInterpolations # Required to interpolate between the controllers
 @named fb  = Blocks.Add(k2=-1)
 @named ref = Blocks.Square(frequency=1/6, amplitude=0.5, offset=0.5, start_time=1)
@@ -134,7 +133,7 @@ eqs = [
 ]
 @named closed_loop = ODESystem(eqs, t, systems=[duffing, Cgs, fb, ref, F])
 prob = ODEProblem(structural_simplify(closed_loop), [F.xd => 0], (0.0, 8.0))
-sol = solve(prob, Rodas5P(), abstol=1e-8, reltol=1e-8, initializealg=NoInit())
+sol = solve(prob, Rodas5P(), abstol=1e-8, reltol=1e-8, initializealg=SciMLBase.NoInit())
 plot!(sol, idxs=[duffing.y.u, duffing.u.u], l=(2, :red), lab="Gain scheduled")
 plot!(sol, idxs=F.output.u, l=(1, :black, :dash, 0.5), lab="Ref")
 ```
@@ -155,7 +154,7 @@ The generated code starts by defining the interpolation vector `xs`, this variab
 We can linearize around a trajectory obtained from `solve` using the function [`trajectory_ss`](@ref). We provide it with a vector of time points along the trajectory at which to linearize, and in this case we specify the inputs and outputs to linearize between as analysis points `r` and `y`.
 ```@example BATCHLIN
 timepoints = 0:0.01:8
-Ps2, ssys = trajectory_ss(closed_loop, closed_loop.r, closed_loop.y, sol; t=timepoints)
+Ps2, ssys = trajectory_ss(closed_loop, closed_loop.r, closed_loop.y, sol; t=timepoints, initialize=true, verbose=true)
 bodeplot(Ps2, w, legend=false)
 ```
 

docs/src/index.md

@@ -139,7 +139,7 @@ ModelingToolkit has facilities for symbolic linearization that can be used on su
 We start by building a system mode, we'll use a model of two masses connected by a flexible transmission
 ```@example LINEAIZE_SYMBOLIC
 using ControlSystemsMTK, ControlSystemsBase
-using ModelingToolkit, OrdinaryDiffEq, LinearAlgebra
+using ModelingToolkit, LinearAlgebra
 using ModelingToolkitStandardLibrary.Mechanical.Rotational
 using ModelingToolkitStandardLibrary.Blocks: Sine
 using ModelingToolkit: connect
@@ -178,7 +178,8 @@ model = SystemModel() |> complete
 ### Numeric linearization
 We can linearize this model numerically using `named_ss`, this produces a `NamedStateSpace{Continuous, Float64}`
 ```@example LINEAIZE_SYMBOLIC
-lsys = named_ss(model, [model.torque.tau.u], [model.inertia1.phi, model.inertia2.phi], op = Dict(model.torque.tau.u => 0))
+op = Dict(model.inertia1.flange_b.phi => 0.0, model.torque.tau.u => 0)
+lsys = named_ss(model, [model.torque.tau.u], [model.inertia1.phi, model.inertia2.phi]; op)
 ```
 ### Symbolic linearization
 If we instead call `linearize_symbolic` and pass the jacobians into `ss`, we get a `StateSpace{Continuous, Num}`

src/ode_system.jl

@@ -154,19 +154,22 @@ end
 
 
 """
-    RobustAndOptimalControl.named_ss(sys::ModelingToolkit.AbstractTimeDependentSystem, inputs, outputs; kwargs...)
+    RobustAndOptimalControl.named_ss(sys::ModelingToolkit.AbstractTimeDependentSystem, inputs, outputs; descriptor=true, kwargs...)
 
 Convert an `ODESystem` to a `NamedStateSpace` using linearization. `inputs, outputs` are vectors of variables determining the inputs and outputs respectively. See docstring of `ModelingToolkit.linearize` for more info on `kwargs`.
 
-This method automatically converts systems that MTK has failed to produce a proper form for into a proper linear statespace system. Learn more about how that is done here:
+If `descriptor = true` (default), this method automatically converts systems that MTK has failed to produce a proper form for into a proper linear statespace system using the method described here:
 https://juliacontrol.github.io/ControlSystemsMTK.jl/dev/#Internals:-Transformation-of-non-proper-models-to-proper-statespace-form
+If `descriptor = false`, the system is instead converted to a statespace realization using `sys[:,uinds] + sys[:,duinds]*tf('s')`, which tends to result in a larger realization on which the user may want to call `minreal(sys, tol)` with a carefully selected tolerance.
+
 
 See also [`ModelingToolkit.linearize`](@ref) which is the lower-level function called internally. The functions [`get_named_sensitivity`](@ref), [`get_named_comp_sensitivity`](@ref), [`get_named_looptransfer`](@ref) similarily provide convenient ways to compute sensitivity functions while retaining signal names in the same way as `named_ss`. The corresponding lower-level functions `get_sensitivity`, `get_comp_sensitivity` and `get_looptransfer` are available in ModelingToolkitStandardLibrary.Blocks and are documented in [MTKstdlib: Linear analysis](https://docs.sciml.ai/ModelingToolkitStandardLibrary/stable/API/linear_analysis/).
 """
 function RobustAndOptimalControl.named_ss(
     sys::ModelingToolkit.AbstractTimeDependentSystem,
     inputs,
     outputs;
+    descriptor = true,
     kwargs...,
 )
 
@@ -206,7 +209,7 @@ function RobustAndOptimalControl.named_ss(
         # This indicates that input derivatives are present
         duinds = findall(any(!iszero, eachcol(matrices.B[:, nu+1:end]))) .+ nu
         u2du = (1:nu) .=> duinds # This maps inputs to their derivatives
-        lsys = causal_simplification(matrices, u2du)
+        lsys = causal_simplification(matrices, u2du; descriptor)
     else
         lsys = ss(matrices...)
     end
@@ -219,26 +222,33 @@ function RobustAndOptimalControl.named_ss(
     )
 end
 
-function causal_simplification(sys, u2duinds::Vector{Pair{Int, Int}})
+function causal_simplification(sys, u2duinds::Vector{Pair{Int, Int}}; descriptor=true)
+    fm(x) = convert(Matrix{Float64}, x)
     nx = size(sys.A, 1)
     ny = size(sys.C, 1)
     ndu = length(u2duinds)
     nu = size(sys.B, 2) - ndu
     u_with_du_inds = first.(u2duinds)
     duinds = last.(u2duinds)
-    B̄ = sys.B[:, duinds]
     B = sys.B[:, 1:nu]
-    Iu = u_with_du_inds .== (1:nu)'
-    E = [I(nx) -B̄; zeros(ndu, nx+ndu)]
-
-    fm(x) = convert(Matrix{Float64}, x)
+    B̄ = sys.B[:, duinds]
+    D = sys.D[:, 1:nu]
+    D̄ = sys.D[:, duinds]
+    iszero(fm(D̄)) || error("Nonzero feedthrough matrix from input derivative not supported")
+    if descriptor
+        Iu = u_with_du_inds .== (1:nu)'
+        E = [I(nx) -B̄; zeros(ndu, nx+ndu)]
 
-    Ae = cat(sys.A, -I(ndu), dims=(1,2))
-    Be = [B; Iu]
-    Ce = [fm(sys.C) zeros(ny, ndu)]
-    De = fm(sys.D[:, 1:nu])
-    dsys = dss(Ae, E, Be, Ce, De)
-    ss(RobustAndOptimalControl.DescriptorSystems.dss2ss(dsys)[1])
+        Ae = cat(sys.A, -I(ndu), dims=(1,2))
+        # Ae[1:nx, nx+1:end] .= B
+        Be = [B; Iu]
+        Ce = [fm(sys.C) zeros(ny, ndu)]
+        De = fm(D)
+        dsys = dss(Ae, E, Be, Ce, De)
+        return ss(RobustAndOptimalControl.DescriptorSystems.dss2ss(dsys)[1])
+    else
+        ss(sys.A, B, sys.C, D) + ss(sys.A, B̄, sys.C, D̄)*tf('s')
+    end
 end
 
 for f in [:sensitivity, :comp_sensitivity, :looptransfer]
@@ -513,7 +523,11 @@ Linearize `sys` around the trajectory `sol` at times `t`. Returns a vector of `S
 function trajectory_ss(sys, inputs, outputs, sol; t = _max_100(sol.t), allow_input_derivatives = false, fuzzer = nothing, verbose = true, kwargs...)
     maximum(t) > maximum(sol.t) && @warn("The maximum time in `t`: $(maximum(t)), is larger than the maximum time in `sol.t`: $(maximum(sol.t)).")
     minimum(t) < minimum(sol.t) && @warn("The minimum time in `t`: $(minimum(t)), is smaller than the minimum time in `sol.t`: $(minimum(sol.t)).")
-    lin_fun, ssys = linearization_function(sys, inputs, outputs; kwargs...)
+
+    # NOTE: we call linearization_funciton twice :( The first call is to get x=unknowns(ssys), the second call provides the operating points.
+    # lin_fun, ssys = linearization_function(sys, inputs, outputs; warn_initialize_determined = false, kwargs...)
+    lin_fun, ssys = linearization_function(sys, inputs, outputs; warn_initialize_determined = false, kwargs...)
+
     x = unknowns(ssys)
     defs = ModelingToolkit.defaults(sys)
     ops = map(t) do ti
@@ -526,8 +540,10 @@ function trajectory_ss(sys, inputs, outputs, sol; t = _max_100(sol.t), allow_inp
         ops = reduce(vcat, opsv)
         t = repeat(t, inner = length(ops) ÷ length(t))
     end
+    # lin_fun, ssys = linearization_function(sys, inputs, outputs; op=ops[1], initialize, kwargs...)
     lins = map(zip(ops, t)) do (op, t)
         linearize(ssys, lin_fun; op, t, allow_input_derivatives)
+        # linearize(sys, inputs, outputs; op, t, allow_input_derivatives, initialize=false)[1]
     end
     (; linsystems = [ss(l...) for l in lins], ssys, ops)
 end
@@ -662,7 +678,7 @@ function GainScheduledStateSpace(systems, vt; interpolator, x = zeros(systems[1]
     @variables x(t)[1:nx]=x [
         description = "State variables of gain-scheduled statespace system $name",
     ]
-    @variables v(t) = 0 [
+    @variables v(t) [
         description = "Scheduling variable of gain-scheduled statespace system $name",
     ]
 

test/test_ODESystem.jl

@@ -1,7 +1,8 @@
 using ControlSystemsMTK,
-    ControlSystemsBase, ModelingToolkit, RobustAndOptimalControl
+    ControlSystemsBase, ModelingToolkit, RobustAndOptimalControl, Test
 import ModelingToolkitStandardLibrary.Blocks as Blocks
 using OrdinaryDiffEqNonlinearSolve, OrdinaryDiffEqRosenbrock
+using LinearAlgebra
 conn = ModelingToolkit.connect
 connect = ModelingToolkit.connect
 ## Test SISO (single input, single output) system
@@ -40,33 +41,34 @@ sol2 = solve(prob, Rodas5())
 isinteractive() && plot(sol2)
 
 Pc = complete(P)
-Q = ControlSystemsMTK.build_quadratic_cost_matrix(Pc, [Pc.input.u], [Pc.x[1] => 2.0])
+op = Dict(Pc.input.u => 0.0)
+Q = ControlSystemsMTK.build_quadratic_cost_matrix(Pc, [Pc.input.u], [Pc.x[1] => 2.0]; op)
 @test Q[] ≈ 2.0
 
-Q = ControlSystemsMTK.build_quadratic_cost_matrix(Pc, [Pc.input.u], [Pc.output.u => 2.0])
+Q = ControlSystemsMTK.build_quadratic_cost_matrix(Pc, [Pc.input.u], [Pc.output.u => 2.0]; op)
 @test Q[] ≈ 2.0
 
 #Mix states and outputs
-Q = ControlSystemsMTK.build_quadratic_cost_matrix(Pc, [Pc.input.u], [Pc.x[1] => 2.0, Pc.output.u => 3])
+Q = ControlSystemsMTK.build_quadratic_cost_matrix(Pc, [Pc.input.u], [Pc.x[1] => 2.0, Pc.output.u => 3]; op)
 @test Q[] ≈ 2.0 + 3
 
-matrices, ssys = linearize(Pc, [Pc.input.u], [Pc.output.u])
+matrices, ssys = linearize(Pc, [Pc.input.u], [Pc.output.u]; op)
 
 Q = ControlSystemsMTK.build_quadratic_cost_matrix(matrices, ssys, [Pc.x[1] => 2.0])
 @test Q[] ≈ 2.0
 
 Q = ControlSystemsMTK.build_quadratic_cost_matrix(matrices, ssys, [Pc.output.u => 2.0])
 @test Q[] ≈ 2.0
 
-P1 = ss(Pc, [Pc.input.u], [Pc.output.u])
+P1 = ss(Pc, [Pc.input.u], [Pc.output.u]; op)
 @test P1 == P0
 
 
 
 # === Go the other way, ODESystem -> StateSpace ================================
 x = unknowns(P) # I haven't figured out a good way to access states, so this is a bit manual and ugly
 @unpack input, output = P
-P02_named = named_ss(P, [input.u], [output.u])
+P02_named = named_ss(P, [input.u], [output.u]; op)
 @test P02_named.x == [Symbol("(x(t))[1]")]
 @test P02_named.u == [Symbol("input₊u(t)")]
 @test P02_named.y == [Symbol("output₊u(t)")]
@@ -76,7 +78,7 @@ P02 = ss(P02_named)
 
 # same for controller
 x = unknowns(C)
-@nonamespace C02 = named_ss(C, [C.input], [C.output])
+@nonamespace C02 = named_ss(C, [C.input], [C.output]; op)
 @test ss(C02) == C0
 
 
@@ -216,7 +218,8 @@ end
 
 model = SystemModel() |> complete
 
-lsys = named_ss(model, [model.torque.tau.u], [model.inertia1.phi, model.inertia2.phi])
+op = Dict(model.inertia1.flange_b.phi => 0.0, model.torque.tau.u => 0)
+lsys = named_ss(model, [model.torque.tau.u], [model.inertia1.phi, model.inertia2.phi]; op)
 @test -1000 ∈ lsys.A
 @test -10 ∈ lsys.A
 @test 1000 ∈ lsys.A
@@ -295,7 +298,7 @@ using ModelingToolkitStandardLibrary.Mechanical.TranslationalPosition
 using Test
 
 using ControlSystemsMTK
-using ControlSystemsMTK.ControlSystemsBase: sminreal, minreal, poles
+using ControlSystemsMTK.ControlSystemsBase: sminreal, minreal, poles, ss, tf
 connect = ModelingToolkit.connect
 
 @independent_variables t
@@ -348,3 +351,38 @@ op2[cart.f] = 0
 @test G.nx == 4
 @test G.nu == length(lin_inputs)
 @test G.ny == length(lin_outputs)
+
+## Test difficult `named_ss` simplification
+using ControlSystemsMTK, ControlSystemsBase, RobustAndOptimalControl
+lsys = (A = [0.0 2.778983834717109e8 1.4122312296634873e6 0.0; 0.0 0.0 0.0 0.037848975765016724; 0.0 24.837541148074962 0.12622006230897712 0.0; -0.0 -4.620724819774693 -0.023481719514324866 -0.6841991610512456], B = [-5.042589978197361e8 0.0; -0.0 0.0; -45.068824982602656 -0.0; 8.384511049369085 54.98555939873381], C = [0.0 0.0 0.954929658551372 0.0], D = [0.0 0.0])
+
+# lsys = (A = [-0.0075449237853825925 1.6716817118020731e-6 0.0; 1864.7356343162514 -0.4131578457122937 0.0; 0.011864343330426718 -2.6287085638214332e-6 0.0], B = [0.0 0.0; 0.0 52566.418015009294; 0.0 0.3284546792274811], C = [1.4683007399899215e8 0.0 0.0], D = [-9.157636303058283e7 0.0])
+
+G = ControlSystemsMTK.causal_simplification(lsys, [1=>2])
+G2 = ControlSystemsMTK.causal_simplification(lsys, [1=>2], descriptor=false)
+G2 = minreal(G2, 1e-12)
+
+@test dcgain(G, 1e-5)[] ≈ dcgain(G2, 1e-5)[] rtol=1e-3
+@test freqresp(G, 1)[] ≈ freqresp(G2, 1)[]
+@test freqresp(G, 10)[] ≈ freqresp(G2, 10)[]
+
+z = 0.462726166562343204837317130554462562
+
+@test minimum(abs, tzeros(G) .- z) < sqrt(eps())
+@test minimum(abs, tzeros(G2) .- z) < sqrt(eps())
+
+# using GenericSchur
+
+Gb = balance_statespace(G)[1]
+Gb = minreal(Gb, 1e-8)
+@test Gb.nx == 2
+@test minimum(abs, tzeros(Gb) .- z) < sqrt(eps())
+
+w = exp10.(LinRange(-12, 2, 2000))
+# ControlSystemsBase.bodeplot([G, G2, minreal(G, 1e-8)], w)
+
+
+##
+
+# S = schur(A,B)
+# V = eigvecs(S)

test/test_batchlin.jl

@@ -1,6 +1,8 @@
 using ControlSystemsMTK, ModelingToolkit, RobustAndOptimalControl, MonteCarloMeasurements
 using OrdinaryDiffEqNonlinearSolve, OrdinaryDiffEqRosenbrock
+using ModelingToolkitStandardLibrary.Blocks
 using ModelingToolkit: getdefault
+using Test
 unsafe_comparisons(true)
 
 # Create a model
@@ -24,7 +26,7 @@ sample_within_bounds((l, u)) = (u - l) * rand() + l
 # Create a vector of operating points
 N = 10
 xs = range(getbounds(x)[1], getbounds(x)[2], length=N)
-ops = Dict.(x .=> xs)
+ops = Dict.(u.u => 0, x .=> xs)
 
 inputs, outputs = [u.u], [y.u]
 Ps, ssys = batch_ss(duffing, inputs, outputs , ops)
@@ -55,11 +57,11 @@ import ModelingToolkitStandardLibrary.Blocks
 
 
 closed_loop_eqs = [
-    connect(ref.output, :r, F.input)
-    connect(F.output, fb.input1)
-    connect(duffing.y, :y, fb.input2)
-    connect(fb.output, C.input)
-    connect(C.output, duffing.u)
+    ModelingToolkit.connect(ref.output, :r, F.input)
+    ModelingToolkit.connect(F.output, fb.input1)
+    ModelingToolkit.connect(duffing.y, :y, fb.input2)
+    ModelingToolkit.connect(fb.output, C.input)
+    ModelingToolkit.connect(C.output, duffing.u)
 ]
 
 @named closed_loop = ODESystem(closed_loop_eqs, t, systems=[duffing, C, fb, ref, F])
@@ -73,6 +75,7 @@ sol = solve(prob, Rodas5P(), abstol=1e-8, reltol=1e-8)
 
 time = 0:0.1:8
 inputs, outputs = [duffing.u.u], [duffing.y.u]
+op = Dict(u.u => 0)
 Ps2, ssys = trajectory_ss(closed_loop, closed_loop.r, closed_loop.y, sol; t=time)
 @test length(Ps2) == length(time)
 # bodeplot(Ps2)