Dude, I would like to thank you from the bottom of my heart for your solution to your problem. I needed to solve the same problem with the equations of motion of a body with 6 degrees of freedom, by hand it would have been very long. I divided the system of original differential equations into matrices and then multiplied again and everything matches the original ones.

Here is an example of my steps as I obtained each matrix:

q = [x; y; z; phi; theta; psi]
qdot = [x_dot;y_dot;z_dot;phi_dot;theta_dot;psi_dot]
qddot = [x_ddot;y_ddot;z_ddot;phi_ddot;theta_ddot;psi_ddot]

% initial equations of motion
eqns = transpose([eqddx, eqddy, eqddz, eqddphi, eqddtheta, eqddpsi])

%Mass and inertia matrix (you can also use the matlab function)
[MM, zbytek] = equationsToMatrix(eqns, qddot)

%Coriolis force and drag force matrix
[C_G, zbytek2] = equationsToMatrix_abatea(-1*zbytek, qdot)

%my some inputs in differential equations
inputs = [Thrust; Tau_phi; Tau_theta; Tau_psi];

%Matrix for inputs L and gravity Q
[L, Q] = equationsToMatrix_abatea(-1*zbytek2, inputs)
Q = -1*Q;

Multiplication for comparison

vychozi = expand(eqns)
roznasobeni = expand( MM*qddot + C_G*qdot + Q == L*inputs)