Merge pull request #1816 from SciML/fb/lin_dae

fix matrix sizes and input order in linearize

Author: baggepinnen

src/inputoutput.jl

@@ -218,7 +218,8 @@ function generate_control_function(sys::AbstractODESystem, inputs = unbound_inpu
     f, dvs, ps
 end
 
-function inputs_to_parameters!(state::TransformationState, check_bound = true)
+function inputs_to_parameters!(state::TransformationState, io)
+    check_bound = io === nothing
     @unpack structure, fullvars, sys = state
     @unpack var_to_diff, graph, solvable_graph = structure
     @assert solvable_graph === nothing
@@ -274,6 +275,17 @@ function inputs_to_parameters!(state::TransformationState, check_bound = true)
     @set! sys.eqs = map(Base.Fix2(substitute, input_to_parameters), equations(sys))
     @set! sys.states = setdiff(states(sys), keys(input_to_parameters))
     ps = parameters(sys)
+
+    if io !== nothing
+        # Change order of new parameters to correspond to user-provided order in argument `inputs`
+        d = Dict{Any, Int}()
+        for (i, inp) in enumerate(new_parameters)
+            d[inp] = i
+        end
+        permutation = [d[i] for i in io.inputs]
+        new_parameters = new_parameters[permutation]
+    end
+
     @set! sys.ps = [ps; new_parameters]
 
     @set! state.sys = sys

src/systems/abstractsystem.jl

@@ -968,7 +968,7 @@ function structural_simplify(sys::AbstractSystem, io = nothing; simplify = false
     state = TearingState(sys)
     has_io = io !== nothing
     has_io && markio!(state, io...)
-    state, input_idxs = inputs_to_parameters!(state, !has_io)
+    state, input_idxs = inputs_to_parameters!(state, io)
     sys = alias_elimination!(state)
     # TODO: avoid construct `TearingState` again.
     state = TearingState(sys)
@@ -984,7 +984,7 @@ end
 
 function io_preprocessing(sys::AbstractSystem, inputs,
                           outputs; simplify = false, kwargs...)
-    sys, input_idxs = structural_simplify(sys, (inputs, outputs); simplify, kwargs...)
+    sys, input_idxs = structural_simplify(sys, (; inputs, outputs); simplify, kwargs...)
 
     eqs = equations(sys)
     alg_start_idx = findfirst(!isdiffeq, eqs)
@@ -1201,6 +1201,8 @@ function linearize(sys, lin_fun; t = 0.0, op = Dict(), allow_input_derivatives =
     nz = size(f_z, 2)
     ny = size(h_x, 1)
 
+    D = h_u
+
     if isempty(g_z)
         A = f_x
         B = f_u
@@ -1216,20 +1218,20 @@ function linearize(sys, lin_fun; t = 0.0, op = Dict(), allow_input_derivatives =
         A = [f_x f_z
              gzgx*f_x gzgx*f_z]
         B = [f_u
-             zeros(nz, nu)]
+             gzgx * f_u] # The cited paper has zeros in the bottom block, see derivation in https://github.com/SciML/ModelingToolkit.jl/pull/1691 for the correct formula
+
         C = [h_x h_z]
         Bs = -(gz \ g_u) # This equation differ from the cited paper, the paper is likely wrong since their equaiton leads to a dimension mismatch.
         if !iszero(Bs)
             if !allow_input_derivatives
                 der_inds = findall(vec(any(!=(0), Bs, dims = 1)))
-                error("Input derivatives appeared in expressions (-g_z\\g_u != 0), the following inputs appeared differentiated: $(inputs(sys)[der_inds]). Call `linear_staespace` with keyword argument `allow_input_derivatives = true` to allow this and have the returned `B` matrix be of double width ($(2nu)), where the last $nu inputs are the derivatives of the first $nu inputs.")
+                error("Input derivatives appeared in expressions (-g_z\\g_u != 0), the following inputs appeared differentiated: $(inputs(sys)[der_inds]). Call `linear_statespace` with keyword argument `allow_input_derivatives = true` to allow this and have the returned `B` matrix be of double width ($(2nu)), where the last $nu inputs are the derivatives of the first $nu inputs.")
             end
-            B = [B Bs]
+            B = [B [zeros(nx, nu); Bs]]
+            D = [D zeros(ny, nu)]
         end
     end
 
-    D = h_u
-
     (; A, B, C, D)
 end