clear all
syms x1 x2 x3 x4 z1 z2 z3 z4 M g L K I J u v
syms x1p x2p x3p x4p z1p z2p z3p z4p 

xp=[x1p;x2p;x3p;x4p]
f = [x2 ; ...
    -M*g*L/I*sin(x1)-K/I*(x1-x3) ;...
    x4 ;...
    K/J*(x1-x3) ];
G = [0;0;0;1/J];

ad1 = jacobian(G,[x1 x2 x3 x4])*f-jacobian(f,[x1 x2 x3 x4])*G;
ad2 = jacobian(ad1,[x1 x2 x3 x4])*f-jacobian(f,[x1 x2 x3 x4])*ad1;
ad3 = jacobian(ad2,[x1 x2 x3 x4])*f-jacobian(f,[x1 x2 x3 x4])*ad2;
cont = [G ad1 ad2 ad3]

xp_x = f+G*u;

z=[z1;z2;z3;z4];

z_x(1,1) = x1;
z_x(2,1) = jacobian(z_x(1),[x1 x2 x3 x4])*f
z_x(3,1) = jacobian(z_x(2),[x1 x2 x3 x4])*f
z_x(4,1) = jacobian(z_x(3),[x1 x2 x3 x4])*f

c1 = jacobian(z_x(4,1),[x1 x2 x3 x4])*f
c2 = jacobian(z_x(4,1),[x1 x2 x3 x4])*G

u = 1/c2 * (-c1 + v)


zp_x = jacobian(z_x,[x1 x2 x3 x4])*xp_x;




x_z = solve(z_x-z);

simplify(subs(zp_x(1),[x1 x2 x3 x4],[x_z.x1 x_z.x2 x_z.x3 x_z.x4])) 
simplify(subs(zp_x(2),[x1 x2 x3 x4],[x_z.x1 x_z.x2 x_z.x3 x_z.x4]))
simplify(subs(zp_x(3),[x1 x2 x3 x4],[x_z.x1 x_z.x2 x_z.x3 x_z.x4]))
collect(simplify(subs(zp_x(4),[x1 x2 x3 x4],[x_z.x1 x_z.x2 x_z.x3 x_z.x4])),u)