Damian, Hello! Putting matrices C =diag(1,...,1) and D=zeros(.,.) get Your system of equations. Simulink State-Space block allows you to solve linear control problems without much hassle. I can send a training file where the problem of oscillations of a double spring pendulum is solved by Simulink. Matrix A: Size [4x4]. I hope You solve Your problem by analogy. Simulink also has nonlinear blocks x'=f (x, u, t) to solve your control. While there is little free time to learn more about your script. Don't take offense at me. Advice is much easier to give than to program.
Sincerely Yours, Pavel Osipov.