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

댓글 수: 6

We need t, x_sv_array, u_veh as inputs and the function "airplane_coefficients" to test your code.
Sam
Sam 2025년 12월 17일
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
Torsten 2025년 12월 17일
편집: Torsten 2025년 12월 17일
% 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
Sam 2025년 12월 17일
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
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
Torsten 2025년 12월 17일
편집: Torsten 2025년 12월 18일
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.

댓글을 달려면 로그인하십시오.

답변 (1개)

Ritam
Ritam 2025년 12월 23일
Assuming your supporting functions are correct, the core issue appears to be in the 4th Runge-Kutta's method implementation: you compute
f = airplane_coefficients(X, U_vec)
four times with effectively the same state and control inputs. In classical fourth‑order Runge–Kutta's method, each of the four slopes must be evaluated at different intermediate states and times, using updated inputs derived from the preceding slope
k1 at (t,X,u(t))
k2at (t+dt2,X+dt2k1,u(t+dt2))
k3 at (t+dt2,X+dt2k1,u(t+dt2))
k4at (t+dt,X+dtk3,u(t+dt))
As a result, you should adjust the calls to "airplane_coefficients" so that each evaluation uses the appropriate intermediate state and time‑aligned control vector (rather than the initial X and U_vec). Making this change will align your integrator with RK4’s scheme and should resolve the “no data displayed” symptom by producing nontrivial state updates. Additionally, ensure the returned state is assigned back to your output (e.g., X_next into x_sv_array), and verify vector shapes (column orientation) to avoid silent plotting issues.

제품

릴리스

R2021b

태그

질문:

Sam
2025년 12월 17일

답변:

2025년 12월 23일

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by