Rearranjed controller parameters functions due to Matlab limitations