Numerical integration of the missile dynamic model
24 Ansichten (letzte 30 Tage)
Ältere Kommentare anzeigen
MOHANDAREZKI AKTOUF
am 5 Jul. 2021
Kommentiert: Alan Stevens
am 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 Kommentare
Akzeptierte Antwort
Alan Stevens
am 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 Kommentare
Alan Stevens
am 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!).
Weitere Antworten (0)
Siehe auch
Kategorien
Mehr zu Instrument Control Toolbox finden Sie in Help Center und File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!