Numerical integration of the missile dynamic model
23 views (last 30 days)
Show older comments
MOHANDAREZKI AKTOUF
on 5 Jul 2021
Commented: Alan Stevens
on 6 Jul 2021
Hello everyone,
I want to integrate the dynamic model of a 6DOF flying missile (using ode45 and RK4 methods) and plot the trajectory of the missile , this model contains 12 nonlinear ode's and discrete data, external forces and moments are calcualted by a seperate function and when I launch the simulation I get this error :
Warning: Failure at t=5.051155e+00. Unable to meet integration tolerances without reducing the step size below the smallest
value allowed (1.421085e-14) at time t.
> In ode45 (line 360)
In ode45_integration (line 11) .
I would appreciate your help and your suggestions. Best regards
My functions are :
%%%% Dynamic model integration using ode45 %%%%
t0 = 0;
tf = 15;
h = 0.01;
timerange = t0:h:tf;
IC = [0;0;1;0;deg2rad(-18);0;13;0;0;0;0;0];
%% Integration par ode45 %%%
[t,Y] = ode45(@(t,Y) MDD_Missile(t,Y),timerange,IC);
plot(Y(:,1),Y(:,3))
xlabel('range');
ylabel('Altitude(m)');
legend('altitude en fonction de la porteé');
grid on;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function dYdt = MDD_Missile(t,Y)
%%% Missile discrete data %%%
time = [0; 0.3; 0.6; 1.2; 1.8; 2.4; 4.2; 5.2];
m = [11.25; 11.16; 11.06; 10.82; 10.58; 10.38; 10.16; 10.15];
TR = [0; 570; 650; 750; 770; 650; 50; 0];
rxA = [0.565; 0.555; 0.544; 0.519; 0.493; 0.471; 0.447; 0.446];
Ixx = [0.0252; 0.025; 0.0248; 0.0244; 0.0239; 0.0235; 0.0231; 0.0231];
Iyy = [0.985; 0.979; 0.973; 0.958; 0.942; 0.929; 0.915; 0.914];
Izz = [0.985; 0.979; 0.973; 0.958; 0.942; 0.929; 0.915; 0.914];
%%% Interpolation data-set with t %%%
m = interp1(time,m,t);
Ixx = interp1(time,Ixx,t);
Iyy = interp1(time,Iyy,t);
Izz = interp1(time,Izz,t);
TR = interp1(time,TR,t);
rxA = interp1(time,rxA,t);
IG = [Ixx 0 0;
0 Iyy 0;
0 0 Izz];
%% Command parameters (Fins orientation angles) %%
sigma_9g = deg2rad (0);
sigma_10g = deg2rad (0);
sigma_11g = deg2rad (0);
sigma_12g = deg2rad (0);
%% Call of fonction calculting external forces and moments acting on the missile %%
ForcesMoments_Aero = F_M (Y,m,TR,rxA,sigma_9g,sigma_10g,sigma_11g,sigma_12g);
Fx = ForcesMoments_Aero(1);
Fy = ForcesMoments_Aero(2);
Fz = ForcesMoments_Aero(3);
L = ForcesMoments_Aero(4);
M = ForcesMoments_Aero(5);
N = ForcesMoments_Aero(6);
%%% 12 différential equation of motion of the missile %%%
VF = [Fx; Fy; Fz];
VM = [L; M; N];
Mat_oRb = [cos(Y(5))*cos(Y(6)) cos(Y(6))*sin(Y(5))*sin(Y(4))-sin(Y(6))*cos(Y(4)) cos(Y(6))*sin(Y(5))*cos(Y(4))+sin(Y(6))*sin(Y(4));
sin(Y(6))*cos(Y(5)) sin(Y(6))*sin(Y(5))*sin(Y(4))+cos(Y(6))*cos(Y(5)) sin(Y(6))*sin(Y(5))*cos(Y(4))-cos(Y(6))*sin(Y(4));
-sin(Y(5)) cos(Y(5))*sin(Y(4)) cos(Y(5))*cos(Y(4))];
Mat_H = [1 sin(Y(4))*tan(Y(5)) cos(Y(4))*tan(Y(5));
0 cos(Y(4)) -sin(Y(4));
0 sin(Y(4))/cos(Y(5)) cos(Y(4))/cos(Y(5))];
Mat = [0 -Y(12) Y(11);
Y(12) 0 -Y(10);
-Y(11) Y(10) 0];
V_xyz = Mat_oRb*[Y(7); Y(8); Y(9)]; %[dxdt; dydt; dzdt]
V_euler = Mat_H*[Y(10); Y(11); Y(12)]; %[dalphadt; dbetadt; dgammadt]
V_uvw = (VF/m)-(Mat*[Y(7); Y(8); Y(9)]); %[dudt; dvdt; dwdt]
V_pqr = inv(IG)*(VM-(Mat*IG*[Y(10);Y(11);Y(12)])); % [dpdt; dqdt; drdt]
dYdt = zeros(12,1);
dYdt(1) = V_xyz(1);
dYdt(2) = V_xyz(2);
dYdt(3) = V_xyz(3);
dYdt(4) = V_euler(1);
dYdt(5) = V_euler(2);
dYdt(6) = V_euler(3);
dYdt(7) = V_uvw(1);
dYdt(8) = V_uvw(2);
dYdt(9) = V_uvw(3);
dYdt(10) = V_pqr(1);
dYdt(11) = V_pqr(2);
dYdt(12) = V_pqr(3);
dYdt = dYdt(:);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function ForcesMoments_Aero = F_M (Y,m,TR,rxA,sigma_9g,sigma_10g,sigma_11g,sigma_12g)
rxT = - 0.46;
ryT = 0;
rzT = 0;
rT = [rxT;
ryT;
rzT];
rho = 1.225;
g = -9.81;
D = 0.127;
% rxA = 0.12528;
ryA = 0;
rzA = 0;
CX0 = 0.255;
CX2 = 0.484;
CNA = 3.298;
CLP = -0.042;
CMQ = -1.8;
CLa = 1.683;
CD0a = 0.004;
CD2a = 0.268;
CLg = 0.905;
CD0g = 0.004;
CD2g = 0.111;
rgm = 0.0985;
lgm = -0.154;
lgc = -0.465;
rgc = 0.065;
%% Calcul des forces %%
%% Force de gravite %%
FG = g*m*[-sin(Y(5));
cos(Y(5))*sin(Y(4));
cos(Y(4))*cos(Y(5))];
%% Force aerodynamique du corps missile %%
V = sqrt(Y(7)^2+Y(8)^2+Y(9)^2);
FA =-(pi*rho*V^2*D^2/8)*[CX0+CX2*(Y(8)^2+Y(9)^2)/V^2;
CNA*(Y(8)/V);
CNA*(Y(9)/V)];
%% Calcul des moments %%
%% Moments des forces aerodynamique du corps missile %%
rA = [rxA;
ryA;
rzA];
Moment_A = cross(rA,FA);
%% Moments aerodynamiques non reguliers %%
Moment_UA = (pi*rho*V*D^4/16)*[CLP*Y(10);
CMQ*Y(11);
CMQ*Y(12)];
%% Forces est moments des surfaces additionnelles %%
%% Les ailes %%
FW_a = zeros(3,1);
Moment_a = zeros(3,1);
for j = 1 : 8
phi_a = (j - 1) * (pi/4);
rx_a = lgm;
ry_a = rgm * cos(phi_a);
rz_a = rgm * sin(phi_a);
SLi_a = 0.00175;
gamma_a = 0;
sigma_a = 0;
r_a=[rx_a;
ry_a;
rz_a];
phi_i = phi_a;
gamma_i = gamma_a;
K = Ti (phi_i,gamma_i)' * ([Y(7);Y(8);Y(9)]+([0 -Y(12) Y(11);Y(12) 0 -Y(10);-Y(11) Y(10) 0]*r_a));
V_i = sqrt(K(1)^2+K(2)^2+K(3)^2);
alpha_a = sigma_a + atan(K(3)/K(1));
CL_a = alpha_a * CLa;
CD_a = CD0a + CD2a * alpha_a^2;
Ma = (0.5*rho*SLi_a*V_i^2)*Ti (phi_i,gamma_i)*[CL_a*sin(alpha_a-sigma_a)-CD_a*cos(alpha_a-sigma_a);
0;
-CL_a*sin(alpha_a-sigma_a)-CD_a*cos(alpha_a-sigma_a)];
FW_a = FW_a + Ma;
Moment_a = Moment_a + cross(r_a,Ma);
end
%% Les ailerons %%
V_orientation = [sigma_9g;
sigma_10g;
sigma_11g;
sigma_12g];
FW_g = zeros(3,1);
Moment_g = zeros(3,1);
F_TVC = zeros(3,1);
Moment_TVC = zeros(3,1);
for j = 9 : 12
phi_g = (pi/4) + (j-8) * (pi/2);
rx_g = lgc;
ry_g = rgc * cos(phi_g);
rz_g = rgc * sin(phi_g);
SLi_g = 0.0035;
gamma_g = pi/6;
sigma_g = V_orientation(j-8);
r_g=[rx_g;
ry_g;
rz_g];
phi_i= phi_g;
gamma_i = gamma_g;
K = Ti (phi_i,gamma_i)'*([Y(7);Y(8);Y(9)]+[0 -Y(12) Y(11);Y(12) 0 -Y(10);-Y(11) Y(10) 0]*r_g);
V_i = sqrt(K(1)^2+K(2)^2+K(3)^2);
alpha_g = sigma_g + atan(K(3)/K(1));
CL_g = alpha_g * CLg;
CD_g = CD0g + CD2g * alpha_g^2;
Mg = (0.5*rho* SLi_g *V_i^2)*Ti (phi_i,gamma_i)*[CL_g*sin(alpha_g-sigma_g)-CD_g*cos(alpha_g-sigma_g);
0;
-CL_g*sin(alpha_g-sigma_g)-CD_g*cos(alpha_g-sigma_g);];
FW_g = FW_g + Mg;
Moment_g = Moment_g + cross(r_g,Mg);
F_TVC = F_TVC + (TR/4)*[cos(sigma_g);
sin(phi_g)*sin(sigma_g);
-cos(phi_g)*sin(sigma_g)];
Moment_TVC = Moment_TVC + cross(rT,F_TVC);
end
FW = FW_a + FW_g;
Moment_W = Moment_a + Moment_g;
%% Forces et moments totales %%
F_aero = FG + FA + FW+ F_TVC;
F_aero = F_aero(:);
Fx = F_aero(1);
Fy = F_aero(2);
Fz = F_aero(3);
M_aero = Moment_A + Moment_TVC + Moment_W + Moment_UA;
M_aero = M_aero(:);
L = M_aero(1);
M = M_aero(2);
N = M_aero(3);
ForcesMoments_Aero = [Fx; Fy; Fz; L; M; N];
end
3 Comments
Accepted Answer
Alan Stevens
on 5 Jul 2021
I don't know the actual problem, but if you look at your values of Y against time you will see that some of them are clearly diverging towards +/- infinity. That is what is causing Matab to complain. You need to investigate that in some detail.
%%%% Dynamic model integration using ode45 %%%%
t0 = 0;
tf = 5.09;
h = 0.1;
timerange = [t0 tf]; %t0:h:tf;
IC = [0;0;1;0;deg2rad(-18);0;13;0;0;0;0;0];
%% Integration par ode45 %%%
[t,Y] = ode15s(@(t,Y) MDD_Missile(t,Y),timerange,IC);
plot(t,Y)
xlabel('time')
ylabel('Y')
% xlabel('range');
% ylabel('Altitude(m)');
% legend('altitude en fonction de la porteé');
grid on;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function dYdt = MDD_Missile(t,Y)
%%% Missile discrete data %%%
time = [0; 0.3; 0.6; 1.2; 1.8; 2.4; 4.2; 5.2; 15];
mm = [11.25; 11.16; 11.06; 10.82; 10.58; 10.38; 10.16; 10.15; 10.15];
TRR = [0; 570; 650; 750; 770; 650; 50; 0; 0];
rxAA = [0.565; 0.555; 0.544; 0.519; 0.493; 0.471; 0.447; 0.446; 0.446];
Ixxx = [0.0252; 0.025; 0.0248; 0.0244; 0.0239; 0.0235; 0.0231; 0.0231; 0.231];
Iyyy = [0.985; 0.979; 0.973; 0.958; 0.942; 0.929; 0.915; 0.914; 0.914];
Izzz = [0.985; 0.979; 0.973; 0.958; 0.942; 0.929; 0.915; 0.914; 0.914];
%%% Interpolation data-set with t %%%
m = interp1(time,mm,t);
Ixx = interp1(time,Ixxx,t);
Iyy = interp1(time,Iyyy,t);
Izz = interp1(time,Izzz,t);
TR = interp1(time,TRR,t);
rxA = interp1(time,rxAA,t);
IG = [Ixx 0 0;
0 Iyy 0;
0 0 Izz];
%% Command parameters (Fins orientation angles) %%
sigma_9g = deg2rad (0);
sigma_10g = deg2rad (0);
sigma_11g = deg2rad (0);
sigma_12g = deg2rad (0);
%% Call of fonction calculting external forces and moments acting on the missile %%
ForcesMoments_Aero = F_M (Y,m,TR,rxA,sigma_9g,sigma_10g,sigma_11g,sigma_12g);
Fx = ForcesMoments_Aero(1);
Fy = ForcesMoments_Aero(2);
Fz = ForcesMoments_Aero(3);
L = ForcesMoments_Aero(4);
M = ForcesMoments_Aero(5);
N = ForcesMoments_Aero(6);
%%% 12 différential equation of motion of the missile %%%
VF = [Fx; Fy; Fz];
VM = [L; M; N];
Mat_oRb = [cos(Y(5))*cos(Y(6)) cos(Y(6))*sin(Y(5))*sin(Y(4))-sin(Y(6))*cos(Y(4)) cos(Y(6))*sin(Y(5))*cos(Y(4))+sin(Y(6))*sin(Y(4));
sin(Y(6))*cos(Y(5)) sin(Y(6))*sin(Y(5))*sin(Y(4))+cos(Y(6))*cos(Y(5)) sin(Y(6))*sin(Y(5))*cos(Y(4))-cos(Y(6))*sin(Y(4));
-sin(Y(5)) cos(Y(5))*sin(Y(4)) cos(Y(5))*cos(Y(4))];
Mat_H = [1 sin(Y(4))*tan(Y(5)) cos(Y(4))*tan(Y(5));
0 cos(Y(4)) -sin(Y(4));
0 sin(Y(4))/cos(Y(5)) cos(Y(4))/cos(Y(5))];
Mat = [0 -Y(12) Y(11);
Y(12) 0 -Y(10);
-Y(11) Y(10) 0];
V_xyz = Mat_oRb*[Y(7); Y(8); Y(9)]; %[dxdt; dydt; dzdt]
V_euler = Mat_H*[Y(10); Y(11); Y(12)]; %[dalphadt; dbetadt; dgammadt]
V_uvw = (VF/m)-(Mat*[Y(7); Y(8); Y(9)]); %[dudt; dvdt; dwdt]
V_pqr = IG\(VM-(Mat*IG*[Y(10);Y(11);Y(12)])); % [dpdt; dqdt; drdt]
dYdt = zeros(12,1);
dYdt(1) = V_xyz(1);
dYdt(2) = V_xyz(2);
dYdt(3) = V_xyz(3);
dYdt(4) = V_euler(1);
dYdt(5) = V_euler(2);
dYdt(6) = V_euler(3);
dYdt(7) = V_uvw(1);
dYdt(8) = V_uvw(2);
dYdt(9) = V_uvw(3);
dYdt(10) = V_pqr(1);
dYdt(11) = V_pqr(2);
dYdt(12) = V_pqr(3);
dYdt = dYdt(:);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function ForcesMoments_Aero = F_M (Y,m,TR,rxA,sigma_9g,sigma_10g,sigma_11g,sigma_12g)
rxT = - 0.46;
ryT = 0;
rzT = 0;
rT = [rxT;
ryT;
rzT];
rho = 1.225;
g = -9.81;
D = 0.127;
% rxA = 0.12528;
ryA = 0;
rzA = 0;
CX0 = 0.255;
CX2 = 0.484;
CNA = 3.298;
CLP = -0.042;
CMQ = -1.8;
CLa = 1.683;
CD0a = 0.004;
CD2a = 0.268;
CLg = 0.905;
CD0g = 0.004;
CD2g = 0.111;
rgm = 0.0985;
lgm = -0.154;
lgc = -0.465;
rgc = 0.065;
%% Calcul des forces %%
%% Force de gravite %%
FG = g*m*[-sin(Y(5));
cos(Y(5))*sin(Y(4));
cos(Y(4))*cos(Y(5))];
%% Force aerodynamique du corps missile %%
V = sqrt(Y(7)^2+Y(8)^2+Y(9)^2);
FA =-(pi*rho*V^2*D^2/8)*[CX0+CX2*(Y(8)^2+Y(9)^2)/V^2;
CNA*(Y(8)/V);
CNA*(Y(9)/V)];
%% Calcul des moments %%
%% Moments des forces aerodynamique du corps missile %%
rA = [rxA;
ryA;
rzA];
Moment_A = cross(rA,FA);
%% Moments aerodynamiques non reguliers %%
Moment_UA = (pi*rho*V*D^4/16)*[CLP*Y(10);
CMQ*Y(11);
CMQ*Y(12)];
%% Forces est moments des surfaces additionnelles %%
%% Les ailes %%
FW_a = zeros(3,1);
Moment_a = zeros(3,1);
for j = 1 : 8
phi_a = (j - 1) * (pi/4);
rx_a = lgm;
ry_a = rgm * cos(phi_a);
rz_a = rgm * sin(phi_a);
SLi_a = 0.00175;
gamma_a = 0;
sigma_a = 0;
r_a=[rx_a;
ry_a;
rz_a];
phi_i = phi_a;
gamma_i = gamma_a;
K = Ti(phi_i,gamma_i)' * ([Y(7);Y(8);Y(9)]+([0 -Y(12) Y(11);Y(12) 0 -Y(10);-Y(11) Y(10) 0]*r_a));
V_i = sqrt(K(1)^2+K(2)^2+K(3)^2);
alpha_a = sigma_a + atan(K(3)/K(1));
CL_a = alpha_a * CLa;
CD_a = CD0a + CD2a * alpha_a^2;
Ma = (0.5*rho*SLi_a*V_i^2)*Ti (phi_i,gamma_i)*[CL_a*sin(alpha_a-sigma_a)-CD_a*cos(alpha_a-sigma_a);
0;
-CL_a*sin(alpha_a-sigma_a)-CD_a*cos(alpha_a-sigma_a)];
FW_a = FW_a + Ma;
Moment_a = Moment_a + cross(r_a,Ma);
end
%% Les ailerons %%
V_orientation = [sigma_9g;
sigma_10g;
sigma_11g;
sigma_12g];
FW_g = zeros(3,1);
Moment_g = zeros(3,1);
F_TVC = zeros(3,1);
Moment_TVC = zeros(3,1);
for j = 9 : 12
phi_g = (pi/4) + (j-8) * (pi/2);
rx_g = lgc;
ry_g = rgc * cos(phi_g);
rz_g = rgc * sin(phi_g);
SLi_g = 0.0035;
gamma_g = pi/6;
sigma_g = V_orientation(j-8);
r_g=[rx_g;
ry_g;
rz_g];
phi_i= phi_g;
gamma_i = gamma_g;
K = Ti(phi_i,gamma_i)'*([Y(7);Y(8);Y(9)]+[0 -Y(12) Y(11);Y(12) 0 -Y(10);-Y(11) Y(10) 0]*r_g);
V_i = sqrt(K(1)^2+K(2)^2+K(3)^2);
alpha_g = sigma_g + atan(K(3)/K(1));
CL_g = alpha_g * CLg;
CD_g = CD0g + CD2g * alpha_g^2;
Mg = (0.5*rho* SLi_g *V_i^2)*Ti (phi_i,gamma_i)*[CL_g*sin(alpha_g-sigma_g)-CD_g*cos(alpha_g-sigma_g);
0;
-CL_g*sin(alpha_g-sigma_g)-CD_g*cos(alpha_g-sigma_g);];
FW_g = FW_g + Mg;
Moment_g = Moment_g + cross(r_g,Mg);
F_TVC = F_TVC + (TR/4)*[cos(sigma_g);
sin(phi_g)*sin(sigma_g);
-cos(phi_g)*sin(sigma_g)];
Moment_TVC = Moment_TVC + cross(rT,F_TVC);
end
FW = FW_a + FW_g;
Moment_W = Moment_a + Moment_g;
%% Forces et moments totales %%
F_aero = FG + FA + FW+ F_TVC;
F_aero = F_aero(:);
Fx = F_aero(1);
Fy = F_aero(2);
Fz = F_aero(3);
M_aero = Moment_A + Moment_TVC + Moment_W + Moment_UA;
M_aero = M_aero(:);
L = M_aero(1);
M = M_aero(2);
N = M_aero(3);
ForcesMoments_Aero = [Fx; Fy; Fz; L; M; N];
end
function Matrice_passage = Ti (phi_i,gamma_i)
Sgi = sin (gamma_i);
Cgi = cos (gamma_i);
Spi = sin (phi_i);
Cpi = cos (phi_i);
Matrice_passage = [Cgi -Sgi 0;
Cpi*Sgi Cpi*Cgi -Spi;
Spi*Sgi Spi*Cgi Cpi];
end
3 Comments
Alan Stevens
on 6 Jul 2021
Your original end time went beyond the limits of your interpolation vectors. I thought this might be a cause of the problem - it wasn't!
Your program is too complicated or me to work out what is going on from the listing. If you were able to upload a mathematical model of the system I might take a look (though I promise nothing!).
More Answers (0)
See Also
Categories
Find more on Instrument Control Toolbox in Help Center and File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!