Foreach model is working. Parametrized controller is diverging.
The parametrized controller works fine.

It was just a Copy-Paste error in the attitude controller. :( I did a reshape of r,p,y reshaping always r.

By the way now it is working fine but there is still a problem. I had to fix the R matrix size and now when I want to simulate just 1 robot I have the problem that I don't have a 3D R matrix. I will try to solve this soon.