It doesn't work that way because you have created an algebraic loop error where
depends y5 that is your
, which depends on y6 that is
from the beginning. I'll try to explain how to solve it without using difficult math or matrix operations. Here, the governing equations are given by
Equation 5:
Equation 6:
.Equation 6 can be rearranged to become
so that it can substituted into Equation 5 to eliminate 
and then be simplified to become
... Equation (7).Note the the right-hand side of Equation 7 does not have the term
. Subtituting Equation 7 into the simplified Equation 6 yields
... Equation (8).Now you can rewrite the mbd ode function in state-space form correctly. Note that the ODEs should have only 4 states (instead of 6 states):
If you find my technical explanation is helpful, please vote and accept the answer.