论文复现：三维比例导引

1 论文

欧阳中辉, 刘家祺, 张龙杰,等. 基于矢量运算的三维真比例导引弹道仿真[J]. 弹箭与制导学报, 2013, 33(1):53-56.

三维比例导引，矢量动力学

2 程序

主函数

clear;close all;
global N
N=4;
rt=[7000,10,20]'; rm=[0 0 0]';
vt=[-300 0 0]'; vm=[700 400 200]';
tf=10; dt=0.01; tspan=0:dt:tf;
state=[rt;vt;rm;vm];
stateout=state;
tout=[0];
[tout,stateout]=ode45('equ',tspan,state);
% 自编固定步长ode45
% for t=0:dt:tf
%     k1=dt*equ(t,state);
%     k2=dt*equ(t+0.5*dt,state+0.5*k1);
%     k3=dt*equ(t+0.5*dt,state+0.5*k2);
%     k4=dt*equ(t+dt,state+k3);
%     state=state+(k1+2*k2+2*k3+k4)/6;
%     stateout=[stateout,state];
%     tout=[tout,t];
% end
% stateout=stateout';
figure
plot3(stateout(:,1),stateout(:,2),stateout(:,3),'r','linewidth',2); hold on;
plot3(stateout(:,7),stateout(:,8),stateout(:,9),'b','linewidth',2); hold on;
grid on;
xlabel('x/m'); ylabel('y/m'); zlabel('z/m');

子函数

function out = equ (t,in)
global N
rt=in(1:3); vt=in(4:6);
rm=in(7:9); vm=in(10:12);
r=rt-rm; vr=vt-vm;
omega=cross(r,vr)/norm(r)^2;
vc=-norm(r.*vr)*r/norm(r)^2;
am=N*cross(vc,omega);
% am=cross(N*dot(r,vr)*r/norm(r)^4,cross(r,vr));
dotrt=vt;
dotvt=[0 0 0]';
dotrm=vm;
dotvm=am;
out=[dotrt;dotvt;dotrm;dotvm];