clc

clear all
% Flight Parameters
c = 3.159 ;
S = 64.0;
rho = 1.225;
m = 19638.23;
AR = 7.22;
H = 2400;
Th = 2000;
g = 9.81;
e = 0.9112;
s = 10.775;

%
%
%
%
%
%

Mean Aerodynamic Chord (meters)

Wing Area (meter square)
Density of Air (kg/meter-cube)
Gross weight (kilograms)
Aspect Ratio
Cruise altitude (meters)
% Engine Thrust (Newtons)
% Gravity (m/sec square)
% span efficiency factor
% semi span

% Moment of inertia (Kg-meter square)

Ixx = 189367.2;
Iyy = 252687.0;
Izz = 415850.9;
Ixz = 11442.0;
% Aerodynamic parameters
% Longitudinal
CL0 = 0;
CLa = 2.6307;
CLq = 4.4134;
CLde = 1.0425;
CD0 = 0.0323;
Cm0 = 0;
Cma = -1.16;
Cmq = -1.1228; % Made n
Cmde = -0.3557;
% Lateral
Cy0 = 0.0;
Cyp = 0.303;
Cyr = 0.727;
Cyb = -1.333;
Cyda = 0.029;
Cydr = 0.191;
Cl0 = 0.0;
Clp = -0.978; % Made n
Clr = 0.418;
Clb = -0.126;
Clda = -0.247;
Cldr = 0.046;
Cn0 = 0;
Cnp = -0.115;
Cnr = -0.495;
Cnb = 0.281;
Cnda = 0.0;
Cndr = -0.166;
% Initial Conditions

u = 296;
v = 0;
w = 0;
p = 0;
q = 0;
r = 0;
phi = 0;
theta = 0;
psi = 0;
alpha = 0;
beta = 0;
t = 0;
h = 0.01; % Time step for Longitudunal Motion
h1 = 0.01; % Time step for Lateral Motion
fid = fopen('sixdof_sim.txt','w'); % Opening the file in writable mode
while t <=
K(1) =
L(1) =
M(1) =
N(1) =
O(1) =
P(1) =
Q(1) =
R(1) =
U(1) =
T(1) =

7 % Begining the while loop

0; % 10 variables for parameters viz. u,v,w,phi,theta,psi,p,q,r,H
0;
0;
0;
0;
0;
0;
0;
0;
0;

for i = 1 : 4
if i <= 3
K(1)= K(i)/2;
L(1)= L(i)/2;
M(1)= M(i)/2;
N(1)= N(i)/2;
O(1)= O(i)/2;
P(1)= P(i)/2;
Q(1)= Q(i)/2;
R(1)= R(i)/2;
U(1)= U(i)/2;
T(1)= T(i)/2;
else
K(1)= K(i);
L(1)= L(i);
M(1)= M(i);
N(1)= N(i);
O(1)= O(i);
P(1)= P(i);
Q(1)= Q(i);
R(1)= R(i);
U(1)= U(i);
T(1)= T(i);
end % (end of for loop)
if t <=
de
da
dr

3
= 0*pi/180;
= 0*pi/180;
= 7*pi/180;

elseif
de
da
dr
elseif
de
da
dr
else
de
da
dr
end
u = u
v = v
w = w
phi =
theta
psi =
p = p
q = q
r = r
H = H

t
=
=
=
t
=
=
=

<= 5
0*pi/180;
0*pi/180;
-7*pi/180;
<=6
0*pi/180;
0*pi/180;
7*pi/180;

= 0*pi/180;
= 0*pi/180;
= -7*pi/180;
+ K(1);
+ L(1);
+ M(1);
phi + N(1);
= theta + O(1);
psi + P(1);
+ Q(1);
+ R(1);
+ U(1);
+ T(1);

V = sqrt(u^2 + v^2 + w^2);

beta = asin(v/V);
alpha = atan(w/u);
dp = 0.5 * rho * V * V;
CL
CD
Cx
Cz
Cm
Cy
Cl
Cn

=
=
=
=
=
=
=
=

CL0 + CLa*alpha + CLq*((q*c)/(2*V))

CD0 + CL^2 / (pi*e*AR);
CL*sin(alpha) - CD*cos(alpha);
-CL*cos(alpha) - CD*sin(alpha);
Cm0 + Cma*alpha + Cmq*((q*c)/(2*V))
Cy0 + Cyp*((p*s)/V) + Cyr*((r*s)/V)
Cl0 + Clp*((p*s)/V) + Clr*((r*s)/V)
Cn0 + Cnp*((p*s)/V) + Cnr*((r*s)/V)

+ CLde*de;

+
+
+
+

Cmde*de;
Cyb*beta + Cyda*da + Cydr*dr;
Clb*beta + Clda*da + Cldr*dr;
Cnb*beta + Cnda*da + Cndr*dr;

K(i+1) = 0 * (-q*w - r*v - g*sin(theta) + ((dp*S)/m)*Cx + Th/m);

L(i+1) = h1 * (-r*u + p*w - g*cos(theta)*sin(phi) + ((dp*S)/m)*Cy);
M(i+1) = 0 * (-p*v + q*u - g*cos(theta)*cos(phi) + ((dp*S)/m)*Cz);
N(i+1) = h1 * (p + q*sin(phi)*tan(theta) + r*cos(phi)*tan(theta));
O(i+1) = 0 * (q*cos(phi) - r*sin(phi));
P(i+1) = h1 * (q*sin(phi)*sec(theta) + r*cos(phi)*sec(theta));
Q(i+1) = h1 * ((dp*S*s*(Izz*Cl+Ixz*Cn) - (q*r*(Ixz^2+Izz^2-Iyy*Izz))) /
(Ixx*Izz-Ixz^2));
R(i+1) = 0 * ((dp*S*c*Cm - (p^2-r^2)*Ixz + p*r*(Izz-Ixx)) / Iyy);
U(i+1) = h1 * ((dp*S*s*(Ixx*Cn+Ixz*Cl) - q*r*Ixz*(Ixx-Iyy+Izz) + p*q*(Ix
z^2-Ixx*Iyy+Ixx^2)) / (Ixx*Izz-Ixz^2));
T(i+1) = 0 * (u*sin(theta) - v*cos(theta)*sin(phi) - w*cos(theta)*cos(ph
i));
end
u = u
v = v
w = w
phi =
theta
psi =

+ (K(2) + (K(3) + K(4))*2 + K(5)) / 6;

+ (L(2) + (L(3) + L(4))*2 + L(5)) / 6;
+ (M(2) + (M(3) + M(4))*2 + M(5)) / 6;
phi + (N(2) + (N(3) + N(4))*2 + N(5)) / 6;
= theta + (O(2) + (O(3) + O(4))*2 + O(5)) / 6;
psi + (P(2) + (P(3) + P(4))*2 + P(5)) / 6;

p
q
r
H

=
=
=
=

p
q
r
H

+
+
+
+

(Q(2)
(R(2)
(U(2)
(T(2)

+
+
+
+

(Q(3)
(R(3)
(U(3)
(T(3)

+
+
+
+

Q(4))*2
R(4))*2
U(4))*2
T(4))*2

+
+
+
+

Q(5))
R(5))
U(5))
T(5))

/
/
/
/

6;
6;
6;
6;

fprintf(fid, '%6.9f %6.9f %6.9f %6.9f %6.9f %6.9f %6.9f %6.9f %6.9f %6.9f %6
.9f %6.9f %6.9f %6.9f %6.9f %6.9f\n', t,alpha,beta,u,v,w,phi,theta,psi,p,q,r,H,d
e,da,dr);
t = t + h;
end
status = fclose(fid);
AA = load('sixdof_sim.txt');
figure(1)
subplot(4,1,1);
plot(AA(:,1),AA(:,3));
title('Lateral Directional Motion Simulation (Three DOF Model)');
ylabel ('\beta , rad');
hold on
subplot(4,1,2);
plot(AA(:,1),AA(:,10));
ylabel ('p , rad/s');
hold on
subplot(4,1,3);
plot(AA(:,1),AA(:,12));
ylabel ('r , rad/s');
hold on
subplot(4,1,4);
plot(AA(:,1),AA(:,15));
ylabel ('\delta_a , rad');
xlabel('Time , sec');
hold off
figure(2)
subplot(4,1,1);
plot(AA(:,1),AA(:,3));
title('Lateral Directional Motion Simulation (Three DOF Model)');
ylabel ('\beta , rad');
hold on
subplot(4,1,2);
plot(AA(:,1),AA(:,10));
ylabel ('p , rad/s');
hold on
subplot(4,1,3);
plot(AA(:,1),AA(:,12));
ylabel ('r , rad/s');
hold on
subplot(4,1,4);
plot(AA(:,1),AA(:,16));
xlabel('Time , sec');
ylabel ('\delta_r , rad');
hold off
%
%
%
%

figure(3)
subplot(3,1,1)
plot(AA(:,1),AA(:,2))
title('Longitudinal Motion Simulation ( Three DOF Model)');

%
%
%
%
%
%
%
%
%
%
%
%
%

ylabel('\alpha')
hold on
subplot(3,1,2)
plot(AA(:,1),AA(:,11))
ylabel('q , rad/s')
hold on
subplot(3,1,3)
plot(AA(:,1),AA(:,14))
xlabel('Time , sec.')
ylabel('\delta_e, rad')
hold off