function u=M_FeedBack(x)
K1=1;
K2=1;
K3=1;
K4=1;
K5=1;
K6=1;
J=1;
f=[-K1*x(1)+K2*x(3)*sin(K5*x(4)),
   -K1*x(2)-K2*x(3)*cos(K5*x(4)),
   -K3*x(1)*sin(K5*x(4))+K3*x(2)*cos(K5*x(4))-K4*x(3)+K6*sin(4*K5*x(4))-x(5)/J,
   x(3)];
a=-K3*f(1)*sin(K5*x(4))-K3*K5*x(1)*x(3)*cos(K5*x(4))...
  +K3*f(2)*cos(K5*x(4))-K3*K5*x(2)*x(3)*sin(K5*x(4))...
  -K4*f(3)+4*K5*K6*x(3)*cos(4*K5*x(4));
z1=x(4);
z2=x(3);
z3=f(3);
p=poly(3*[-1,-1,-1]);
v=-a-p(4)*(z1-x(6))-p(3)*z2-p(2)*z3;
u=[-v/K3*sin(K5*x(4));
    v/K3*cos(K5*x(4))];
