Va0 = (u0^2 + v0^2 + w0^2)^0.5;
G1 = (Jxz * (Jx - Jy + Jz))/ G ;
G2 = (Jz * (Jz - Jy) + (Jxz^2))/ G;
G7 = ((Jx - Jy) * Jx + (Jxz ^ 2)) / G;
num_steps = total_time / time_step;
Pn = load("C:\Users\1537268928E\OneDrive - United States Air Force\Desktop\Pn.mat", "ans").ans;
Pe = load("C:\Users\1537268928E\OneDrive - United States Air Force\Desktop\Pe.mat", "ans").ans;
Pd = load("C:\Users\1537268928E\OneDrive - United States Air Force\Desktop\Pd.mat", "ans").ans;
Phi = load("C:\Users\1537268928E\OneDrive - United States Air Force\Desktop\phi.mat","ans").ans;
Psi = load("C:\Users\1537268928E\OneDrive - United States Air Force\Desktop\psi.mat","ans").ans;
Theta = load("C:\Users\1537268928E\OneDrive - United States Air Force\Desktop\theta.mat","ans").ans;
handle = drawPlaneBody(pn(k),pe(k),pd(k),phi(k),theta(k),psi(k),handle);
function handle = drawPlaneBody(pn,pe,pd,phi,theta,psi,handle)
NED = rotate(NED, phi, theta, psi);
NED = translate(NED, pn, pe, pd);
handle = plot3(XYZ(1,:),XYZ(2,:),XYZ(3,:),'b');
xlabel('X');ylabel('Y');zlabel('Z');
set(handle, 'XData', XYZ(1,:), 'YData',XYZ(2,:), 'ZData',XYZ(3,:));
function XYZ = airplanepoints
function XYZ=rotate(XYZ,phi,theta,psi)
R_pitch = [cos(theta), 0, sin(theta);
-sin(theta), 0, cos(theta)];
R_yaw = [cos(psi), -sin(psi), 0;
R = R_roll*R_pitch*R_yaw;
function XYZ = translate(XYZ, pn, pe, pd)
XYZ = XYZ + repmat([pn;pe;pd],1,size(XYZ,2));