function u = f_RKG(z)

global RKG xOld tau

temp1 =  pendulum(z(1: 4)+xOld);
temp2 =  pendulum(z(5: 8)+xOld);
temp3 =  pendulum(z(9:12)+xOld);

u(1: 4) = z(1: 4) - tau*( RKG(1,1)*temp1 + RKG(1,2)*temp2 + RKG(1,3)*temp3 );
u(5: 8) = z(5: 8) - tau*( RKG(2,1)*temp1 + RKG(2,2)*temp2 + RKG(2,3)*temp3 );
u(9:12) = z(9:12) - tau*( RKG(3,1)*temp1 + RKG(3,2)*temp2 + RKG(3,3)*temp3 );


end

Published with MATLAB® R2012b