fix matrix sizes in linearize

when there are input derivatives present.

Author: baggepinnen

src/systems/abstractsystem.jl

@@ -1198,6 +1198,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
@@ -1213,20 +1215,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]
+
         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