Numerical integration of the missile dynamic model
조회 수: 17 (최근 30일)
이전 댓글 표시
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
채택된 답변
Alan Stevens
2021년 7월 5일
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
Alan Stevens
2021년 7월 6일
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!).
추가 답변 (0개)
참고 항목
카테고리
Help Center 및 File Exchange에서 Instrument Control Toolbox에 대해 자세히 알아보기
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!