4th Order Runge Kutta using Numerical Integration

I am running this code to help me calculate the state space derivatives of an F-18 simulation, right my full code file runs and outputs graphs but I am not getting any data displayed, i believe my issue is stemming from the runge-kutta numerical integration function I built. Any help or suggestion on how to fix this or where I might have gone wrong.

function [t,x_sv_array,u_veh] = num_integration(t, x_sv_array, u_veh)

X = x_sv_array;
U_vec = u_veh;

hrzt = u_veh(1);
ail = u_veh(2);
rdr = u_veh(3);
flaple = u_veh(4);
spbr = u_veh(5);
thrtl = u_veh(6);

t0 = 0;
dt = 0.1;
t2 = 10;

t_hrzt_tab   = [t0 t2].';
t_ail_tab    = [t0 t2].';
t_rdr_tab    = [t0 t2].';
t_flaple_tab = [t0 t2].';
t_spbr_tab   = [t0 t2].';
t_thrtl_tab  = [t0 t2].';

u_hrzt_tab   = [hrzt   hrzt  ].';
u_ail_tab    = [ail    ail   ].';
u_rdr_tab    = [rdr    rdr   ].';
u_flaple_tab = [flaple flaple].';
u_spbr_tab   = [spbr   spbr  ].';
u_thrtl_tab  = [thrtl  thrtl ].';

hrzt = interp1(t_hrzt_tab, u_hrzt_tab,t);
ail = interp1(t_ail_tab, u_ail_tab,t);
rdr = interp1(t_rdr_tab, u_rdr_tab,t);
flaple = interp1(t_flaple_tab, u_flaple_tab,t);
spbr = interp1(t_spbr_tab, u_spbr_tab,t);
thrtl = interp1(t_thrtl_tab, u_thrtl_tab,t);
u_veh = [hrzt ail rdr flaple spbr thrtl].';

f = airplane_coefficients(X,U_vec);
y = x_sv_array + f*dt/3;
z = x_sv_array + f*dt/2;

hrzt = interp1(t_hrzt_tab, u_hrzt_tab,t);
ail = interp1(t_ail_tab, u_ail_tab,t);
rdr = interp1(t_rdr_tab, u_rdr_tab,t);
flaple = interp1(t_flaple_tab, u_flaple_tab,t);
spbr = interp1(t_spbr_tab, u_spbr_tab,t);
thrtl = interp1(t_thrtl_tab, u_thrtl_tab,t);
u_veh = [hrzt ail rdr flaple spbr thrtl].';

f = airplane_coefficients(X,U_vec);
y = y + f*dt/3;
z = x_sv_array + f*dt/2;

hrzt = interp1(t_hrzt_tab, u_hrzt_tab,t);
ail = interp1(t_ail_tab, u_ail_tab,t);
rdr = interp1(t_rdr_tab, u_rdr_tab,t);
flaple = interp1(t_flaple_tab, u_flaple_tab,t);
spbr = interp1(t_spbr_tab, u_spbr_tab,t);
thrtl = interp1(t_thrtl_tab, u_thrtl_tab,t);
u_veh = [hrzt ail rdr flaple spbr thrtl].';

f = airplane_coefficients(X,U_vec);
y = y + f*dt/3;
z = x_sv_array + f*dt/2;
t = t + dt/2;

hrzt = interp1(t_hrzt_tab, u_hrzt_tab,t);
ail = interp1(t_ail_tab, u_ail_tab,t);
rdr = interp1(t_rdr_tab, u_rdr_tab,t);
flaple = interp1(t_flaple_tab, u_flaple_tab,t);
spbr = interp1(t_spbr_tab, u_spbr_tab,t);
thrtl = interp1(t_thrtl_tab, u_thrtl_tab,t);
u_veh = [hrzt ail rdr flaple spbr thrtl].';

f = airplane_coefficients(X,U_vec);
x_sv = y + f*dt/6;


return

end

[Torsten]

We need t, x_sv_array, u_veh as inputs and the function "airplane_coefficients" to test your code.

[Sam]

t0 = 0;

tf = 1;

dt = 0.1;

dtp = 0.01;

t = (t0:dt:tf);

x_sv_array = [x0 y0 z0 u0 v0 w0 phi0 theta0 psi0 p0 q0 r0].';

u_veh = [dh aileron rdr flap_LE spbr throttle].';

x0 = 0; % ft

y0 = 0; % ft

z0 = -25000; % ft

vt0 = 600; % ft/s

alfa0 = (4.064886459773182636 - 0.0);

beta0 = 0;

phi0 = 0;

theta0 = (4.064886459773182636 - 0.0);

psi0 = 0;

u0 = vt0*cos(alfa0)*cos(beta0); % ft/s

v0 = vt0*sin(beta0); % ft/s

w0 = vt0*sin(alfa0)*cos(beta0); % ft/s

p0 = 0;

q0 = 0;

r0 = 0;

dh = -1.409445409076865996 - 0.0;

aileron = 0;

rdr = 0;

flap_LE = 0;

spbr = 0;

throttle = 0.247330306036131597;

and here is the airplone coefficients code block

function [f_veh] = airplane_coefficients(X,U_vec)

format longE

% Given flight conditions

% X input array

g = 32.174;

nm2ft = 5280*1.1516;

Xi_coord = X(1);

Y = X(2);

Z = X(3); % ft

Vt = X(4); % ft/s

alpha = X(5); % degrees

beta = X(6); % degrees

Phi = deg2rad(X(7)); % degrees

Theta = deg2rad(X(8)); % degrees

Psi = deg2rad(X(9)); % degrees

P = deg2rad(X(10)); % deg/s

Q = deg2rad(X(11)); % deg/s

R = deg2rad(X(12)); % deg/s

ALPHA = deg2rad(alpha);

BETA = deg2rad(beta);

% U input Array

hrzt = U_vec(1); % deg

ail = U_vec(2); % deg

rdr = U_vec(3); % deg

flaple = U_vec(4); % deg

spbr = U_vec(5);

thrtl = U_vec(6); % percent throttle

% Velocity components equations

U = Vt * cos(ALPHA)*cos(BETA);

V = Vt * sin(BETA);

W = Vt * sin(ALPHA)*cos(BETA);

% Load aerodynamic lookup tables

run('aero_dat_f16b.m');

run('atmos_dat.m');

run('geom_dat_f16b.m');

run('iner_dat_f16b.m');

run('prop_dat_f16b.m');

% Computing Propulsive force T

% Given flight conditions

V_S = interp1(alt_e_tab, vs_e_tab, -Z);

Mach = Vt / V_S; % Compute Mach number

Delta_Th = U_vec(6); % Throttle setting

Thtl = Delta_Th;

% Compute Power Parameter (ΔP) based on throttle setting

if Thtl <= thtl_ab_on_off

Delta_P = k_thtl_ab_off_1 * Thtl + k_thtl_ab_off_0;

else

Delta_P = k_thtl_ab_on_1 * Thtl + k_thtl_ab_on_0;

end

% Interpolate thrust from the lookup table

T = interp3(mach_prop_tab, alt_prop_tab, powr_prop_tab, thst_alt_mach_powr_tab, Mach, -Z, Delta_P, 'linear');

rho = interp1(alt_e_tab, rho_e_tab, -Z);

q_bar = 0.5 * rho * (Vt)^2;

% X direction force table lookups

cax_alfa_beta_hrzt = interp3(beta_aero_tab, alfa_aero_tab, hrzt_aero_long_tab, cax_alfa_beta_hrzt_tab, beta, alpha, hrzt);

cax_alfa_beta_0 = interp3(beta_aero_tab, alfa_aero_tab, hrzt_aero_long_tab, cax_alfa_beta_hrzt_tab, beta, alpha, 0);

cax_alfa_beta_flaple = interp2(beta_aero_tab, alfa_aero_flaple_tab, cax_alfa_beta_flaple_tab, beta, alpha);

cax_alfa_spbr = interp1(alfa_aero_tab, cax_alfa_spbr_tab, alpha);

cax_alfa_q = interp1(alfa_aero_tab, cax_alfa_q_tab, alpha);

cax_alfa_q_flaple = interp1(alfa_aero_flaple_tab, cax_alfa_q_flaple_tab, alpha);

Cax_FlapLE = cax_alfa_beta_flaple - cax_alfa_beta_0;

% Y direction force table lookups

cay_alfa_beta = interp2(beta_aero_tab, alfa_aero_tab, cay_alfa_beta_tab, beta, alpha);

cay_alfa_beta_flaple = interp2(beta_aero_tab, alfa_aero_flaple_tab, cay_alfa_beta_flaple_tab, beta, alpha);

cay_alfa_beta_ail = interp2(beta_aero_tab, alfa_aero_tab, cay_alfa_beta_ail_tab, beta, alpha);

cay_alfa_beta_ail_flaple = interp2(beta_aero_tab, alfa_aero_flaple_tab, cay_alfa_beta_ail_flaple_tab, beta, alpha);

cay_alfa_beta_rdr = interp2(beta_aero_tab, alfa_aero_tab, cay_alfa_beta_rdr_tab, beta, alpha);

cay_alfa_p = interp1(alfa_aero_tab, cay_alfa_p_tab, alpha);

cay_alfa_p_flaple = interp1(alfa_aero_flaple_tab, cay_alfa_p_flaple_tab, alpha);

cay_alfa_r = interp1(alfa_aero_tab, cay_alfa_r_tab, alpha);

cay_alfa_r_flaple = interp1(alfa_aero_flaple_tab, cay_alfa_r_flaple_tab, alpha);

Cay_FlapLE = cay_alfa_beta_flaple - cay_alfa_beta;

Cay_ail = cay_alfa_beta_ail - cay_alfa_beta;

Cay_ail_FlapLE = cay_alfa_beta_ail_flaple - cay_alfa_beta_flaple - (cay_alfa_beta_ail - cay_alfa_beta);

Cay_rdr = cay_alfa_beta_rdr - cay_alfa_beta;

% Z force table lookups

caz_alfa_beta_hrzt = interp3(beta_aero_tab, alfa_aero_tab, hrzt_aero_long_tab, caz_alfa_beta_hrzt_tab, beta, alpha, hrzt);

caz_alfa_beta_0 = interp3(beta_aero_tab, alfa_aero_tab, hrzt_aero_long_tab, caz_alfa_beta_hrzt_tab, beta, alpha, 0);

caz_alfa_beta_flaple = interp2(beta_aero_tab, alfa_aero_flaple_tab, caz_alfa_beta_flaple_tab, beta, alpha);

caz_alfa_spbr = interp1(alfa_aero_tab, caz_alfa_spbr_tab, alpha);

caz_alfa_q = interp1(alfa_aero_tab, caz_alfa_q_tab, alpha);

caz_alfa_q_flaple = interp1(alfa_aero_flaple_tab, caz_alfa_q_flaple_tab, alpha);

Caz_FlapLE = caz_alfa_beta_flaple - caz_alfa_beta_0;

% X moment forces table lookup

cal_alfa_beta_hrzt = interp3(beta_aero_tab, alfa_aero_tab, hrzt_aero_latdir_tab, cal_alfa_beta_hrzt_tab, beta, alpha, hrzt);

cal_alfa_beta_0 = interp3(beta_aero_tab, alfa_aero_tab, hrzt_aero_latdir_tab, cal_alfa_beta_hrzt_tab, beta, alpha, 0);

cal_alfa_beta_flaple = interp2(beta_aero_tab, alfa_aero_flaple_tab, cal_alfa_beta_flaple_tab, beta, alpha);

cal_alfa_beta = interp1(alfa_aero_tab, cal_alfa_beta_tab, alpha);

cal_alfa_beta_ail = interp2(beta_aero_tab, alfa_aero_tab, cal_alfa_beta_ail_tab, beta, alpha);

cal_alfa_beta_ail_flaple = interp2(beta_aero_tab, alfa_aero_flaple_tab, cal_alfa_beta_ail_flaple_tab, beta, alpha);

cal_alfa_beta_rdr = interp2(beta_aero_tab, alfa_aero_tab, cal_alfa_beta_rdr_tab, beta, alpha);

cal_alfa_p = interp1(alfa_aero_tab, cal_alfa_p_tab, alpha);

cal_alfa_p_flaple = interp1(alfa_aero_flaple_tab, cal_alfa_p_flaple_tab, alpha);

cal_alfa_r = interp1(alfa_aero_tab, cal_alfa_r_tab, alpha);

cal_alfa_r_flaple = interp1(alfa_aero_flaple_tab, cal_alfa_r_flaple_tab, alpha);

Cal_FlapLE = cal_alfa_beta_flaple - cal_alfa_beta_0;

Cal_Ail = cal_alfa_beta_ail - cal_alfa_beta_0;

Cal_Ail_FlapLE = cal_alfa_beta_ail_flaple - cal_alfa_beta_flaple - (cal_alfa_beta_ail - cal_alfa_beta_0);

Cal_Rdr = cal_alfa_beta_rdr - cal_alfa_beta_0;

% Y Moment forces table lookup

cam_alfa_beta_hrzt = interp3(beta_aero_tab, alfa_aero_tab, hrzt_aero_long_tab, cam_alfa_beta_hrzt_tab, beta, alpha, hrzt);

cam_alfa_beta_0 = interp3(beta_aero_tab, alfa_aero_tab, hrzt_aero_long_tab, cam_alfa_beta_hrzt_tab, beta, alpha, 0);

cam_alfa_beta_flaple = interp2(beta_aero_tab, alfa_aero_flaple_tab, cam_alfa_beta_flaple_tab, beta, alpha);

cam_alfa_spbr = interp1(alfa_aero_tab, cam_alfa_spbr_tab, alpha);

cam_alfa_q = interp1(alfa_aero_tab, cam_alfa_q_tab, alpha);

cam_alfa_q_flaple = interp1(alfa_aero_flaple_tab, cam_alfa_q_flaple_tab, alpha);

cam_alfa = interp1(alfa_aero_tab, cam_alfa_tab, alpha);

cam_alfa_hrzt = interp2(hrzt_aero_long_cama_tab, alfa_aero_tab, cam_alfa_hrzt_tab, hrzt, alpha);

mu_camabh = interp1(hrzt_aero_long_tab, mu_camabh_tab, hrzt);

Cam_FlapLE = cam_alfa_beta_flaple - cam_alfa_beta_0;

% Z moment Forces table lookup

can_alfa_beta_hrzt = interp3(beta_aero_tab, alfa_aero_tab, hrzt_aero_latdir_tab, can_alfa_beta_hrzt_tab, beta, alpha, hrzt);

can_alfa_beta_0 = interp3(beta_aero_tab, alfa_aero_tab, hrzt_aero_latdir_tab, can_alfa_beta_hrzt_tab, beta, alpha, 0);

can_alfa_beta_flaple = interp2(beta_aero_tab, alfa_aero_flaple_tab, can_alfa_beta_flaple_tab, beta, alpha);

can_alfa_beta = interp1(alfa_aero_tab, can_alfa_beta_tab, alpha);

can_alfa_beta_ail = interp2(beta_aero_tab, alfa_aero_tab, can_alfa_beta_ail_tab, beta, alpha);

can_alfa_beta_ail_flaple = interp2(beta_aero_tab, alfa_aero_flaple_tab, can_alfa_beta_ail_flaple_tab, beta, alpha);

can_alfa_beta_rdr = interp2(beta_aero_tab, alfa_aero_tab, can_alfa_beta_rdr_tab, beta, alpha);

can_alfa_p = interp1(alfa_aero_tab, can_alfa_p_tab, alpha);

can_alfa_p_flaple = interp1(alfa_aero_flaple_tab, can_alfa_p_flaple_tab, alpha);

can_alfa_r = interp1(alfa_aero_tab, can_alfa_r_tab, alpha);

can_alfa_r_flaple = interp1(alfa_aero_flaple_tab, can_alfa_r_flaple_tab, alpha);

Can_FlapLE = can_alfa_beta_flaple - can_alfa_beta_0;

Can_Ail = can_alfa_beta_ail - can_alfa_beta_0;

Can_Ail_FlapLE = can_alfa_beta_ail_flaple - can_alfa_beta_flaple - (can_alfa_beta_ail - can_alfa_beta_0);

Can_Rdr = can_alfa_beta_rdr - can_alfa_beta_0;

% Compute final aerodynamic coefficients

Cax = cax_alfa_beta_hrzt + Cax_FlapLE*(1-flaple/25) + cax_alfa_spbr*(spbr/60) ...

+ (cax_alfa_q + cax_alfa_q_flaple*(1-flaple/25))*(cbar*Q)/(2*Vt);

Cay = cay_alfa_beta + Cay_FlapLE*(1-flaple/25) ...

+ (Cay_ail + Cay_ail_FlapLE*(1-flaple/25))*(ail/20) + Cay_rdr*(rdr/30) ...

+ (cay_alfa_p + cay_alfa_p_flaple*(1-flaple/25))*(bbar*P)/(2*Vt) + (cay_alfa_r + cay_alfa_r_flaple*(1-flaple/25))*(bbar*R)/(2*Vt);

Caz = caz_alfa_beta_hrzt + Caz_FlapLE*(1-flaple/25) + caz_alfa_spbr*(spbr/60) ...

+ (caz_alfa_q + caz_alfa_q_flaple*(1-flaple/25))*(cbar*Q)/(2*Vt);

Cal = cal_alfa_beta_hrzt + Cal_FlapLE*(1-flaple/25) + cal_alfa_beta*deg2rad(beta)...

+ (Cal_Ail + Cal_Ail_FlapLE*(1-flaple/25))*(ail/20) + Cal_Rdr*(rdr/30) ...

+ (cal_alfa_p + cal_alfa_p_flaple*(1-flaple/25))*(bbar*P)/(2*Vt) + (cal_alfa_r + cal_alfa_r_flaple*(1-flaple/25))*(bbar*R)/(2*Vt);

Cam = cam_alfa_beta_hrzt*mu_camabh + Cam_FlapLE*(1-flaple/25) + cam_alfa_spbr*(spbr/60) ...

+ (cam_alfa_q + cam_alfa_q_flaple*(1-flaple/25))*(cbar*Q)/(2*Vt) ...

+ cam_alfa + cam_alfa_hrzt + Caz*(xcmbr-xcmb);

Can = can_alfa_beta_hrzt + Can_FlapLE*(1-flaple/25) + can_alfa_beta*deg2rad(beta) ...

+ (Can_Ail + Can_Ail_FlapLE*(1-flaple/25))*(ail/20) + Can_Rdr*(rdr/30) ...

+ (can_alfa_p + can_alfa_p_flaple*(1-flaple/25))*(bbar*P)/(2*Vt) + (can_alfa_r + can_alfa_r_flaple*(1-flaple/25))*(bbar*R)/(2*Vt) - Cay*(xcmbr-xcmb)*cbar/bbar;

% Force conversions from aerodynamic coefficients

Fax = q_bar * sbar * Cax;

Fay = q_bar * sbar * Cay;

Faz = q_bar * sbar * Caz;

Fpx = T;

Fpy = 0;

Fpz = 0;

% Moment conversion from aerodynamic coefficients

LA = q_bar * sbar * bbar * Cal;

MA = q_bar * sbar * cbar * Cam;

NA = q_bar * sbar * bbar * Can;

LP = 0;

MP = 0;

NP = 0;

del = ixx*iyy*izz - ixx*iyz^2 - iyy*izx^2 - izz*ixy^2 - 2*ixy*iyz*izx;

Ixx_i = (iyy*izz - iyz^2) / del;

Iyy_i = (ixx*izz - izx^2) / del;

Izz_i = (ixx*iyy - ixy^2) / del;

Ixy_i = -(izz*ixy + izx*iyz) / del;

Iyz_i = -(ixx*iyz + ixy*izx) / del;

Izx_i = -(iyy*izx + ixy*iyz) / del;

% Gravitational Forces

Fgx = -mass*g*sin(Theta);

Fgy = mass*g*sin(Phi)*cos(Theta);

Fgz = mass*g*cos(Phi)*cos(Theta);

% Total Forces

Fx = Fax + Fpx + Fgx;

Fy = Fay + Fpy + Fgy;

Fz = Faz + Fpz + Fgz;

% Total Moments

L = LA + LP;

M = MA + MP;

N = NA + NP;

% Compute state derivatives for given input parameters

X_dot = (cos(Psi)*cos(Theta)*U + (cos(Psi)*sin(Theta)*sin(Phi) - sin(Psi)*cos(Phi))*V + (cos(Psi)*sin(Theta)*cos(Phi) + sin(Psi)*sin(Phi))*W);

Y_dot = (sin(Psi)*cos(Theta)*U + (sin(Psi)*sin(Theta)*sin(Phi) + cos(Psi)*cos(Phi))*V + (sin(Psi)*sin(Theta)*cos(Phi) - cos(Psi)*sin(Phi))*W);

Z_dot = (-sin(Theta)*U + (cos(Theta)*sin(Phi))*V + (cos(Theta)*cos(Phi))*W);

Phi_dot = 1*P + (sin(Phi)*sin(Theta))/(cos(Theta))*Q + (cos(Phi)*sin(Theta))/(cos(Theta))*R;

Theta_dot = cos(Phi)*Q + -sin(Phi)*R;

Psi_dot = (sin(Phi)/cos(Theta))*Q + (cos(Phi)/cos(Theta))*R;

U_dot = V*R - W*Q + Fx/mass;

V_dot = W*P - U*R + Fy/mass;

W_dot = U*Q - V*P + Fz/mass;

Hxb_d = (iyy - izz)*Q*R + iyz*(Q^2 - R^2) + (izx*Q - ixy*R)*P + L;

Hyb_d = (izz - ixx)*R*P + izx*(R^2 - P^2) + (ixy*R - iyz*P)*Q + M;

Hzb_d = (ixx - iyy)*P*Q + ixy*(P^2 - Q^2) + (iyz*P - izx*Q)*R + N;

P_dot = Ixx_i * Hxb_d - Ixy_i * Hyb_d - Izx_i * Hzb_d;

Q_dot = -Ixy_i * Hxb_d + Iyy_i * Hyb_d - Iyz_i * Hzb_d;

R_dot = -Izx_i * Hxb_d - Iyz_i * Hyb_d + Izz_i * Hzb_d;

% Output final state derivative results

f_veh = [X_dot Y_dot Z_dot U_dot V_dot W_dot Phi_dot Theta_dot Psi_dot P_dot Q_dot R_dot].';

end

[Torsten]

% Load aerodynamic lookup tables 
run('aero_dat_f16b.m');
run('atmos_dat.m');
run('geom_dat_f16b.m');
run('iner_dat_f16b.m');
run('prop_dat_f16b.m');

are missing.

Please test on your computer if the code technically works before posting it.

[Sam]

the code works, im not getting data from the runge-kutta block, i just mainly want to know if it is being implemented correctly or not, I know for a fact the airplane_coefficients code section is 100 percent correct already

[Sam]

here are the missing data files that you requested if this will help with the troubleshooting

t0 = 0;
tf = 1;
dt = 0.1;
dtp = 0.01;
t = (t0:dt:tf);
x0 = 0; % ft
y0 = 0; % ft
z0 = -25000; % ft
vt0 = 600; % ft/s
alfa0 = (4.064886459773182636 - 0.0);
beta0 = 0;
phi0 =  0;
theta0 = (4.064886459773182636 - 0.0);
psi0 = 0;
u0 = vt0*cos(alfa0)*cos(beta0);                 % ft/s
v0 = vt0*sin(beta0);                            % ft/s
w0 = vt0*sin(alfa0)*cos(beta0);                 % ft/s
p0 = 0;
q0 = 0;
r0 = 0;
dh = -1.409445409076865996 - 0.0;
aileron = 0;
rdr = 0;
flap_LE = 0;
spbr = 0;
throttle = 0.247330306036131597;

x_sv_array = [x0 y0 z0 u0 v0 w0 phi0 theta0 psi0 p0 q0 r0].';
u_veh = [dh aileron rdr flap_LE spbr throttle].';
[t,x_sv_array,u_veh] = num_integration(t, x_sv_array, u_veh)

t = 1×11

5.000000000000000e-02 1.500000000000000e-01 2.500000000000000e-01 3.500000000000000e-01 4.500000000000000e-01 5.500000000000000e-01 6.500000000000000e-01 7.500000000000000e-01 8.500000000000001e-01 9.500000000000001e-01 1.050000000000000e+00

<mw-icon class=""></mw-icon>

<mw-icon class=""></mw-icon>

x_sv_array = 12×1

1.0e+00 * 0 0 -2.500000000000000e+04 -3.619177905177962e+02 0 -4.785556528834621e+02 0 4.064886459773183e+00 0 0 0 0

<mw-icon class=""></mw-icon>

<mw-icon class=""></mw-icon>

u_veh = 66×1

-1.409445409076866e+00 -1.409445409076866e+00 -1.409445409076866e+00 -1.409445409076866e+00 -1.409445409076866e+00 -1.409445409076866e+00 -1.409445409076866e+00 -1.409445409076866e+00 -1.409445409076866e+00 -1.409445409076866e+00 -1.409445409076866e+00 0 0 0 0

<mw-icon class=""></mw-icon>

<mw-icon class=""></mw-icon>

function [t,x_sv_array,u_veh] = num_integration(t, x_sv_array, u_veh)
X = x_sv_array;
U_vec = u_veh;
hrzt = u_veh(1);
ail = u_veh(2);
rdr = u_veh(3);
flaple = u_veh(4);
spbr = u_veh(5);
thrtl = u_veh(6);
t0 = 0;
dt = 0.1;
t2 = 10;
t_hrzt_tab   = [t0 t2].';
t_ail_tab    = [t0 t2].';
t_rdr_tab    = [t0 t2].';
t_flaple_tab = [t0 t2].';
t_spbr_tab   = [t0 t2].';
t_thrtl_tab  = [t0 t2].';
u_hrzt_tab   = [hrzt   hrzt  ].';
u_ail_tab    = [ail    ail   ].';
u_rdr_tab    = [rdr    rdr   ].';
u_flaple_tab = [flaple flaple].';
u_spbr_tab   = [spbr   spbr  ].';
u_thrtl_tab  = [thrtl  thrtl ].';
hrzt = interp1(t_hrzt_tab, u_hrzt_tab,t);
ail = interp1(t_ail_tab, u_ail_tab,t);
rdr = interp1(t_rdr_tab, u_rdr_tab,t);
flaple = interp1(t_flaple_tab, u_flaple_tab,t);
spbr = interp1(t_spbr_tab, u_spbr_tab,t);
thrtl = interp1(t_thrtl_tab, u_thrtl_tab,t);
u_veh = [hrzt ail rdr flaple spbr thrtl].';
f = airplane_coefficients(X,U_vec);
y = x_sv_array + f*dt/3;
z = x_sv_array + f*dt/2;
hrzt = interp1(t_hrzt_tab, u_hrzt_tab,t);
ail = interp1(t_ail_tab, u_ail_tab,t);
rdr = interp1(t_rdr_tab, u_rdr_tab,t);
flaple = interp1(t_flaple_tab, u_flaple_tab,t);
spbr = interp1(t_spbr_tab, u_spbr_tab,t);
thrtl = interp1(t_thrtl_tab, u_thrtl_tab,t);
u_veh = [hrzt ail rdr flaple spbr thrtl].';
f = airplane_coefficients(X,U_vec);
y = y + f*dt/3;
z = x_sv_array + f*dt/2;
hrzt = interp1(t_hrzt_tab, u_hrzt_tab,t);
ail = interp1(t_ail_tab, u_ail_tab,t);
rdr = interp1(t_rdr_tab, u_rdr_tab,t);
flaple = interp1(t_flaple_tab, u_flaple_tab,t);
spbr = interp1(t_spbr_tab, u_spbr_tab,t);
thrtl = interp1(t_thrtl_tab, u_thrtl_tab,t);
u_veh = [hrzt ail rdr flaple spbr thrtl].';
f = airplane_coefficients(X,U_vec);
y = y + f*dt/3;
z = x_sv_array + f*dt/2;
t = t + dt/2;
hrzt = interp1(t_hrzt_tab, u_hrzt_tab,t);
ail = interp1(t_ail_tab, u_ail_tab,t);
rdr = interp1(t_rdr_tab, u_rdr_tab,t);
flaple = interp1(t_flaple_tab, u_flaple_tab,t);
spbr = interp1(t_spbr_tab, u_spbr_tab,t);
thrtl = interp1(t_thrtl_tab, u_thrtl_tab,t);
u_veh = [hrzt ail rdr flaple spbr thrtl].';
f = airplane_coefficients(X,U_vec);
x_sv = y + f*dt/6;
return
end

function [f_veh] = airplane_coefficients(X,U_vec)
format longE
% Given flight conditions
% X input array
g = 32.174;
nm2ft = 5280*1.1516;
Xi_coord = X(1);
Y = X(2);
Z = X(3); % ft
Vt = X(4); % ft/s
alpha = X(5); % degrees
beta = X(6); % degrees
Phi = deg2rad(X(7)); % degrees
Theta = deg2rad(X(8)); % degrees
Psi = deg2rad(X(9)); % degrees
P = deg2rad(X(10)); % deg/s
Q = deg2rad(X(11)); % deg/s
R = deg2rad(X(12)); % deg/s
ALPHA = deg2rad(alpha);
BETA = deg2rad(beta);
% U input Array
hrzt = U_vec(1); % deg
ail = U_vec(2); % deg
rdr = U_vec(3); % deg
flaple = U_vec(4); % deg
spbr = U_vec(5);
thrtl = U_vec(6); % percent throttle
% Velocity components equations
U = Vt * cos(ALPHA)*cos(BETA);
V = Vt * sin(BETA);
W = Vt * sin(ALPHA)*cos(BETA);
% Load aerodynamic lookup tables 
run('aero_dat_f16b.m');
run('atmos_dat.m');
run('geom_dat_f16b.m');
run('iner_dat_f16b.m');
run('prop_dat_f16b.m');
% Computing Propulsive force T 
% Given flight conditions
V_S = interp1(alt_e_tab, vs_e_tab, -Z);  
Mach = Vt / V_S; % Compute Mach number
Delta_Th = U_vec(6); % Throttle setting
Thtl = Delta_Th;
% Compute Power Parameter (ΔP) based on throttle setting
if Thtl <= thtl_ab_on_off
    Delta_P = k_thtl_ab_off_1 * Thtl + k_thtl_ab_off_0;
else
    Delta_P = k_thtl_ab_on_1 * Thtl + k_thtl_ab_on_0;
end
% Interpolate thrust from the lookup table
T = interp3(mach_prop_tab, alt_prop_tab, powr_prop_tab, thst_alt_mach_powr_tab, Mach, -Z, Delta_P, 'linear');
rho = interp1(alt_e_tab, rho_e_tab, -Z); 
q_bar = 0.5 * rho * (Vt)^2;
% X direction force table lookups 
cax_alfa_beta_hrzt = interp3(beta_aero_tab, alfa_aero_tab, hrzt_aero_long_tab, cax_alfa_beta_hrzt_tab, beta, alpha, hrzt);
cax_alfa_beta_0 = interp3(beta_aero_tab, alfa_aero_tab, hrzt_aero_long_tab, cax_alfa_beta_hrzt_tab, beta, alpha, 0);
cax_alfa_beta_flaple = interp2(beta_aero_tab, alfa_aero_flaple_tab, cax_alfa_beta_flaple_tab, beta, alpha);
cax_alfa_spbr = interp1(alfa_aero_tab, cax_alfa_spbr_tab, alpha);
cax_alfa_q = interp1(alfa_aero_tab, cax_alfa_q_tab, alpha);
cax_alfa_q_flaple = interp1(alfa_aero_flaple_tab, cax_alfa_q_flaple_tab, alpha);
Cax_FlapLE = cax_alfa_beta_flaple - cax_alfa_beta_0;
% Y direction force table lookups
cay_alfa_beta = interp2(beta_aero_tab, alfa_aero_tab, cay_alfa_beta_tab, beta, alpha);
cay_alfa_beta_flaple = interp2(beta_aero_tab, alfa_aero_flaple_tab, cay_alfa_beta_flaple_tab, beta, alpha); 
cay_alfa_beta_ail = interp2(beta_aero_tab, alfa_aero_tab, cay_alfa_beta_ail_tab, beta, alpha); 
cay_alfa_beta_ail_flaple = interp2(beta_aero_tab, alfa_aero_flaple_tab, cay_alfa_beta_ail_flaple_tab, beta, alpha); 
cay_alfa_beta_rdr = interp2(beta_aero_tab, alfa_aero_tab, cay_alfa_beta_rdr_tab, beta, alpha); 
cay_alfa_p = interp1(alfa_aero_tab, cay_alfa_p_tab, alpha); 
cay_alfa_p_flaple = interp1(alfa_aero_flaple_tab, cay_alfa_p_flaple_tab, alpha); 
cay_alfa_r = interp1(alfa_aero_tab, cay_alfa_r_tab, alpha); 
cay_alfa_r_flaple = interp1(alfa_aero_flaple_tab, cay_alfa_r_flaple_tab, alpha); 
Cay_FlapLE = cay_alfa_beta_flaple     - cay_alfa_beta; 
Cay_ail = cay_alfa_beta_ail        - cay_alfa_beta; 
Cay_ail_FlapLE = cay_alfa_beta_ail_flaple - cay_alfa_beta_flaple - (cay_alfa_beta_ail - cay_alfa_beta); 
Cay_rdr = cay_alfa_beta_rdr        - cay_alfa_beta; 
% Z force table lookups
caz_alfa_beta_hrzt = interp3(beta_aero_tab, alfa_aero_tab, hrzt_aero_long_tab, caz_alfa_beta_hrzt_tab, beta, alpha, hrzt);
caz_alfa_beta_0 = interp3(beta_aero_tab, alfa_aero_tab, hrzt_aero_long_tab, caz_alfa_beta_hrzt_tab, beta, alpha, 0);
caz_alfa_beta_flaple = interp2(beta_aero_tab, alfa_aero_flaple_tab, caz_alfa_beta_flaple_tab, beta, alpha);
caz_alfa_spbr = interp1(alfa_aero_tab, caz_alfa_spbr_tab, alpha);
caz_alfa_q = interp1(alfa_aero_tab, caz_alfa_q_tab, alpha);
caz_alfa_q_flaple = interp1(alfa_aero_flaple_tab, caz_alfa_q_flaple_tab, alpha);
Caz_FlapLE = caz_alfa_beta_flaple - caz_alfa_beta_0;
% X moment forces table lookup
cal_alfa_beta_hrzt = interp3(beta_aero_tab, alfa_aero_tab, hrzt_aero_latdir_tab, cal_alfa_beta_hrzt_tab, beta, alpha, hrzt);
cal_alfa_beta_0 = interp3(beta_aero_tab, alfa_aero_tab, hrzt_aero_latdir_tab, cal_alfa_beta_hrzt_tab, beta, alpha, 0);
cal_alfa_beta_flaple = interp2(beta_aero_tab, alfa_aero_flaple_tab, cal_alfa_beta_flaple_tab, beta, alpha);
cal_alfa_beta = interp1(alfa_aero_tab, cal_alfa_beta_tab, alpha);
cal_alfa_beta_ail = interp2(beta_aero_tab, alfa_aero_tab, cal_alfa_beta_ail_tab, beta, alpha);
cal_alfa_beta_ail_flaple = interp2(beta_aero_tab, alfa_aero_flaple_tab, cal_alfa_beta_ail_flaple_tab, beta, alpha);
cal_alfa_beta_rdr = interp2(beta_aero_tab, alfa_aero_tab, cal_alfa_beta_rdr_tab, beta, alpha);
cal_alfa_p = interp1(alfa_aero_tab, cal_alfa_p_tab, alpha);
cal_alfa_p_flaple = interp1(alfa_aero_flaple_tab, cal_alfa_p_flaple_tab, alpha);
cal_alfa_r = interp1(alfa_aero_tab, cal_alfa_r_tab, alpha);
cal_alfa_r_flaple = interp1(alfa_aero_flaple_tab, cal_alfa_r_flaple_tab, alpha);
Cal_FlapLE = cal_alfa_beta_flaple - cal_alfa_beta_0;
Cal_Ail = cal_alfa_beta_ail - cal_alfa_beta_0;
Cal_Ail_FlapLE = cal_alfa_beta_ail_flaple - cal_alfa_beta_flaple - (cal_alfa_beta_ail - cal_alfa_beta_0);
Cal_Rdr = cal_alfa_beta_rdr - cal_alfa_beta_0;
% Y Moment forces table lookup
cam_alfa_beta_hrzt = interp3(beta_aero_tab, alfa_aero_tab, hrzt_aero_long_tab, cam_alfa_beta_hrzt_tab, beta, alpha, hrzt);
cam_alfa_beta_0 = interp3(beta_aero_tab, alfa_aero_tab, hrzt_aero_long_tab, cam_alfa_beta_hrzt_tab, beta, alpha, 0);
cam_alfa_beta_flaple = interp2(beta_aero_tab, alfa_aero_flaple_tab, cam_alfa_beta_flaple_tab, beta, alpha);
cam_alfa_spbr = interp1(alfa_aero_tab, cam_alfa_spbr_tab, alpha);
cam_alfa_q = interp1(alfa_aero_tab, cam_alfa_q_tab, alpha);
cam_alfa_q_flaple = interp1(alfa_aero_flaple_tab, cam_alfa_q_flaple_tab, alpha);
cam_alfa = interp1(alfa_aero_tab, cam_alfa_tab, alpha);
cam_alfa_hrzt = interp2(hrzt_aero_long_cama_tab, alfa_aero_tab, cam_alfa_hrzt_tab, hrzt, alpha);
mu_camabh = interp1(hrzt_aero_long_tab, mu_camabh_tab, hrzt);
Cam_FlapLE = cam_alfa_beta_flaple - cam_alfa_beta_0;
% Z moment Forces table lookup
can_alfa_beta_hrzt = interp3(beta_aero_tab, alfa_aero_tab, hrzt_aero_latdir_tab, can_alfa_beta_hrzt_tab, beta, alpha, hrzt);
can_alfa_beta_0 = interp3(beta_aero_tab, alfa_aero_tab, hrzt_aero_latdir_tab, can_alfa_beta_hrzt_tab, beta, alpha, 0);
can_alfa_beta_flaple = interp2(beta_aero_tab, alfa_aero_flaple_tab, can_alfa_beta_flaple_tab, beta, alpha);
can_alfa_beta = interp1(alfa_aero_tab, can_alfa_beta_tab, alpha);
can_alfa_beta_ail = interp2(beta_aero_tab, alfa_aero_tab, can_alfa_beta_ail_tab, beta, alpha);
can_alfa_beta_ail_flaple = interp2(beta_aero_tab, alfa_aero_flaple_tab, can_alfa_beta_ail_flaple_tab, beta, alpha);
can_alfa_beta_rdr = interp2(beta_aero_tab, alfa_aero_tab, can_alfa_beta_rdr_tab, beta, alpha);
can_alfa_p = interp1(alfa_aero_tab, can_alfa_p_tab, alpha);
can_alfa_p_flaple = interp1(alfa_aero_flaple_tab, can_alfa_p_flaple_tab, alpha);
can_alfa_r = interp1(alfa_aero_tab, can_alfa_r_tab, alpha);
can_alfa_r_flaple = interp1(alfa_aero_flaple_tab, can_alfa_r_flaple_tab, alpha);
Can_FlapLE = can_alfa_beta_flaple - can_alfa_beta_0;
Can_Ail = can_alfa_beta_ail - can_alfa_beta_0; 
Can_Ail_FlapLE = can_alfa_beta_ail_flaple - can_alfa_beta_flaple - (can_alfa_beta_ail - can_alfa_beta_0); 
Can_Rdr = can_alfa_beta_rdr - can_alfa_beta_0; 
% Compute final aerodynamic coefficients 
Cax = cax_alfa_beta_hrzt + Cax_FlapLE*(1-flaple/25) + cax_alfa_spbr*(spbr/60) ... 
      + (cax_alfa_q + cax_alfa_q_flaple*(1-flaple/25))*(cbar*Q)/(2*Vt); 
Cay = cay_alfa_beta + Cay_FlapLE*(1-flaple/25) ... 
      + (Cay_ail + Cay_ail_FlapLE*(1-flaple/25))*(ail/20) + Cay_rdr*(rdr/30) ...
      + (cay_alfa_p + cay_alfa_p_flaple*(1-flaple/25))*(bbar*P)/(2*Vt) + (cay_alfa_r + cay_alfa_r_flaple*(1-flaple/25))*(bbar*R)/(2*Vt); 
Caz = caz_alfa_beta_hrzt + Caz_FlapLE*(1-flaple/25) + caz_alfa_spbr*(spbr/60) ... 
      + (caz_alfa_q + caz_alfa_q_flaple*(1-flaple/25))*(cbar*Q)/(2*Vt);
Cal = cal_alfa_beta_hrzt + Cal_FlapLE*(1-flaple/25) + cal_alfa_beta*deg2rad(beta)... 
      + (Cal_Ail + Cal_Ail_FlapLE*(1-flaple/25))*(ail/20) + Cal_Rdr*(rdr/30) ... 
      + (cal_alfa_p + cal_alfa_p_flaple*(1-flaple/25))*(bbar*P)/(2*Vt) + (cal_alfa_r + cal_alfa_r_flaple*(1-flaple/25))*(bbar*R)/(2*Vt); 
Cam = cam_alfa_beta_hrzt*mu_camabh + Cam_FlapLE*(1-flaple/25) + cam_alfa_spbr*(spbr/60) ... 
      + (cam_alfa_q + cam_alfa_q_flaple*(1-flaple/25))*(cbar*Q)/(2*Vt) ... 
      + cam_alfa + cam_alfa_hrzt + Caz*(xcmbr-xcmb);
Can = can_alfa_beta_hrzt + Can_FlapLE*(1-flaple/25) + can_alfa_beta*deg2rad(beta) ... 
      + (Can_Ail + Can_Ail_FlapLE*(1-flaple/25))*(ail/20) + Can_Rdr*(rdr/30) ... 
      + (can_alfa_p + can_alfa_p_flaple*(1-flaple/25))*(bbar*P)/(2*Vt) + (can_alfa_r + can_alfa_r_flaple*(1-flaple/25))*(bbar*R)/(2*Vt) - Cay*(xcmbr-xcmb)*cbar/bbar; 
% Force conversions from aerodynamic coefficients
Fax = q_bar * sbar * Cax;
Fay = q_bar * sbar * Cay;
Faz = q_bar * sbar * Caz;
Fpx = T;
Fpy = 0;
Fpz = 0;
% Moment conversion from aerodynamic coefficients
LA = q_bar * sbar * bbar * Cal;
MA = q_bar * sbar * cbar * Cam;
NA = q_bar * sbar * bbar * Can;
LP = 0;
MP = 0;
NP = 0;
del = ixx*iyy*izz - ixx*iyz^2 - iyy*izx^2 - izz*ixy^2 - 2*ixy*iyz*izx;
Ixx_i = (iyy*izz - iyz^2) / del;
Iyy_i = (ixx*izz - izx^2) / del;
Izz_i = (ixx*iyy - ixy^2) / del;
Ixy_i = -(izz*ixy + izx*iyz) / del;
Iyz_i = -(ixx*iyz + ixy*izx) / del;
Izx_i = -(iyy*izx + ixy*iyz) / del;
 
% Gravitational Forces
Fgx = -mass*g*sin(Theta);
Fgy = mass*g*sin(Phi)*cos(Theta);
Fgz = mass*g*cos(Phi)*cos(Theta);
% Total Forces
Fx = Fax + Fpx + Fgx;
Fy = Fay + Fpy + Fgy;
Fz = Faz + Fpz + Fgz;
% Total Moments
L = LA + LP;
M = MA + MP;
N = NA + NP;
% Compute state derivatives for given input parameters
X_dot = (cos(Psi)*cos(Theta)*U + (cos(Psi)*sin(Theta)*sin(Phi) - sin(Psi)*cos(Phi))*V + (cos(Psi)*sin(Theta)*cos(Phi) + sin(Psi)*sin(Phi))*W);
Y_dot = (sin(Psi)*cos(Theta)*U + (sin(Psi)*sin(Theta)*sin(Phi) + cos(Psi)*cos(Phi))*V + (sin(Psi)*sin(Theta)*cos(Phi) - cos(Psi)*sin(Phi))*W);
Z_dot = (-sin(Theta)*U + (cos(Theta)*sin(Phi))*V + (cos(Theta)*cos(Phi))*W);
Phi_dot = 1*P + (sin(Phi)*sin(Theta))/(cos(Theta))*Q + (cos(Phi)*sin(Theta))/(cos(Theta))*R;
Theta_dot = cos(Phi)*Q + -sin(Phi)*R;
Psi_dot = (sin(Phi)/cos(Theta))*Q + (cos(Phi)/cos(Theta))*R;
U_dot = V*R - W*Q + Fx/mass;
V_dot = W*P - U*R + Fy/mass;
W_dot = U*Q - V*P + Fz/mass;
Hxb_d = (iyy - izz)*Q*R + iyz*(Q^2 - R^2) + (izx*Q - ixy*R)*P + L;
Hyb_d = (izz - ixx)*R*P + izx*(R^2 - P^2) + (ixy*R - iyz*P)*Q + M;
Hzb_d = (ixx - iyy)*P*Q + ixy*(P^2 - Q^2) + (iyz*P - izx*Q)*R + N;
P_dot = Ixx_i * Hxb_d - Ixy_i * Hyb_d - Izx_i * Hzb_d;
Q_dot = -Ixy_i * Hxb_d + Iyy_i * Hyb_d - Iyz_i * Hzb_d;
R_dot = -Izx_i * Hxb_d - Iyz_i * Hyb_d + Izz_i * Hzb_d;
% Output final state derivative results
f_veh = [X_dot Y_dot Z_dot U_dot V_dot W_dot Phi_dot Theta_dot Psi_dot P_dot Q_dot R_dot].';
end

[Torsten]

I arranged your code in some way (see above), but I cannot understand how it's meant to work.

E.g. in num_integration, you compute

u_veh = [hrzt ail rdr flaple spbr thrtl].';

several times, but never use it because you call

f = airplane_coefficients(X,U_vec);

with U_vec instead of u_veh.

Further, you define

t0 = 0;
tf = 1;
dt = 0.1;
t = (t0:dt:tf);

in the script part, pass "t" to function "num_integration", but redefine

t0 = 0;
dt = 0.1;
t2 = 10;

therein.