Not finished with modifications. For Each folder created.

I have to finish the modifications. I arrived until the attitude controller. Look into 3Dmatrix x 2Dmatrix multiplication.

With Riccardo I tried the forEach approach and it worked really smoothly. For this reason I created another folder in which this kind of implementation is.