g1_dot= [1 0           0                                                    0;
         0 1 (cos(phi)*tan(the)+sin(phi)*sec(the)^2)     (-sin(phi)*tan(the)+cos(phi)*sec(the)^2);
         0 0        -sin(phi)                                        -cos(phi);
         0 0 (cos(phi)*sec(the)+sin(phi)*sec(the)*tan(the))  (-sin(phi)*sec(the)+cos(phi)*sec(the)*tan(the))];

g2= [ -(cos(phi)*cos(the))/m 0 0 0;
    0 1/Jx 0 0;
    0 0 1/Jy 0;
    0 0 0 1/Jz];

f2 = [g-(Cd/m)*vz;
    Jbarx*q*r;
    Jbary*p*r;
    Jbarz*p*q];


Lamda= diag([1 0 0 0]);

I_theta= diag([1-THETA_hat 1 1 1]);

c1=diag([1 1 1 1]);
c2=diag([1 1 1 1]);
c3=diag([10 10 10 10]);



x1 = [zE ; phi; the; psi];
x1d= [z_d; phi_d; the_d; psi_d];
x1d_dot= [z_d_dot; phi_d_dot; the_d_dot; psi_d_dot];
x1d_ddot= [z_d_ddot; phi_d_ddot; the_d_ddot; psi_d_ddot];

x2= [vz; p; q; r];

z1=x1-x1d;
q1 = (c1*g1)\(c1*x1d_dot - c2*z1);
z2= x2-q1;
z1_dot= c1*g1*z2-c2*z1;
q1_dot= (c1*g1_dot)\(c1*x1d_ddot - c2*z1_dot);
z2=x2-q1;



Omega_bar= (g2*Map*I_theta)\(q1_dot - f2 - (c1*g1)'*z1-c3*z2);


THETA_hat_dot= -gamma*(z2'*g2*Map*Lamda*Omega_bar);