%% Session 2

function dx=NonlinearEq(t,x,AC)

% Initialize
dx=zeros(12,1)
% Parameters
mass=AC.mass;
u0=AC.vel;
delta_u=x(1)-u0;
th0=AC.theta_0;

% Forces (divide by mass)

deltaX=A_long(1,1)*delta_u+A_long(1,2)*x(3);
deltaZ=A_long(2,1)*x(2)+A_long(2,2)*x(3)+(A_long(2,3)-vel)*x(5);
deltaM=A_long(3,1)*delta_u+A_long(3,2)*x(3)+A_long(3,3)*(5);
deltaL=L_v+x(2)+L_p+x(4)+L_r+x(6);
deltaN=N_v+x(2)+N_p+x(4)+N_r+x(6);
deltaY=Y_v+x(2)+Y_p+x(4)+Y_r+x(6);

X=mass*g*sin(th0)+deltaX;
Z=-mass*g*cos(th0)+deltaZ;
M=deltaM;
L=deltaL;
N=deltaN;
Y=deltaY;

dx(1)=X/mass-g*sin(x(8))-x(5)*x(3)+x(6)*x(2);
dx(2)=Y/mass+g*cos(x(8))*sin(x(7))-x(6)*x(1)+x(4)*x(3);
dx(3)=Z/mass+g*cos(x(8))*cos(x(7))-x(4)*x(2)+x(5)*x(1);
dx(4)=((L+Ixz*((N-x(4)*x(5)*(Iyy_b-Ixx)+Ixz*x(5)*x(6))/Izz)-x(5)*x(6)*(Izz-Iyy_b)
+Ixz*x(4)*x(5))/(Ixx-Ixz^2/Izz));
dx(5)=(M-x(4)*x(6)*(Ixx-Izz)-Ixz*(x(4)^2-x(6)^2))/Iyy_b;
dx(6)=(N+Ixz*((L+Ixz*((N-x(4)*x(5)*(Iyy_b-Ixx)+Ixz*x(5)*x(6))/Izz)-x(5)*x(6)*(Izz-
Iyy_b)+Ixz*x(4)*x(5))/(Ixx-Ixz^2/Izz))-x(4)*x(5)*(Iyy_b-Ixx)+Ixz*x(5)*x(6))/Izz;

dx(7)=x(4)+(x(5)*sin(x(7))+x(6)*cos(x(6)))*tan(x(8));
dx(8)=x(5)*cos(x(7))-x(6)*sin(x(7));
dx(9)=(x(5)*sin(x(7))+x(6)*cos(x(7)))*sec(x(8));
dx(10)=x(1)*cos(x(8))*cos(x(9))+x(2)*(sin(x(7))*sin(x(8))*cos(x(9))-
cos(x(7))*sin(x(9)))+x(3)*(cos(x(7))*sin(x(8))*cos(x(9))+sin(x(7))*sin(x(9)))
dx(11)=x(1)*cos(x(8))*sin(x(9))+x(2)*(sin(x(7))*sin(x(8))*sin(x(9))
+cos(x(7))*cos(x(9)))+x(3)*(cos(x(7))*sin(x(8))*sin(x(9))-sin(x(7))*cos(x(9)));
dx(12)=-x(1)*sin(x(8))+x(2)*(sin(x(7))*cos(x(8)))+x(3)*(cos(x(7))*cos(x(8)));

end