czxbikd.m

反空地导弹比例导引三维弹道仿真

%反空地导弹目标比例导引三维弹道仿真
%红色段为自由落体段
%蓝色段为自主控制段
%黑色段为防空导弹弹道

clear;
ae=pi/180;
dt=0.01;
vm=800;
m=1;
g=9.81;
Mx0=0;Mz0=0;My0=0;
pmr(:,1)=[Mx0;Mz0;My0];
jl=1000;

v0=-272;
vt=abs(v0);
h0=5000;%导弹投放高度
hi=4000;%导弹由自由落体转自主控制时的高度    
h1=3000;%预定平飞高度
vx0=v0;
xt0=20000;

t1=sqrt(2*(h0-hi)/g);      %启控点时间


thetat1=atan(-sqrt(2*g*(h0-hi))/v0);   %启控点倾角
x11=xt0+v0*t1;                              %启控点横坐标
y11=h0-0.5*g*t1^2;                       %启控点纵坐标 ，高度


T=0:dt:t1;
x21=xt0+vx0*T;
y21=h0-0.5*g*T.^2;
thetat=-atan(sqrt(2*g*(h0-y21))/v0)*180/pi;
figure(1);plot3(xt0+vx0*T,jl+T*0,h0-0.5*g*T.^2,'r'),hold on,  %自由段坐标变化
%figure(2);plot(T,thetat,'r'),hold on,


%figure(2);plot(T,atan(-sqrt(2*g*(h0-y1))/v0)*180/pi),hold on,            %自由段倾角变化
%自主控制段

ptr(:,1)=[x11;jl;y11];

xi=x11;
n=(h0-hi)/(hi-h1);%h1=h0'
v=sqrt(n*(n+1))*sqrt(2*g*(h0-h1))/v0;  %k
thetat2=atan(-v*(y11-h1)/(h0-h1))*57.3;

thetam=atan((ptr(3,1)-pmr(3,1))./(ptr(1,1)-pmr(1,1)));
psim=atan((ptr(2,1)-pmr(2,1))./(ptr(1,1)-pmr(1,1)));
time=0;
for k=2:4000;
    time=time+dt;
   %r(k-1)=sqrt((ptr(1,k-1)-pmr(1,k-1))^2+(ptr(2,k-1)-pmr(2,k-1))^2+(ptr(3,k-1)-pmr(3,k-1))^2);
    %if r(k-1)<=110              %变步长
      % dt=0.001;
       % end    
    x31=x11+vx0*dt;
    x11=x31;
    y31=h1+(hi-h1)*exp(-v*(x31-xi)/(h0-h1));
    ptr(:,k)=[x31;jl;y31];
    thetat2=atan(-v*(y31-h1)/(h0-h1))*57.3;

    Mx(k)=pmr(1,k-1)+vm*dt*cos(thetam)*cos(psim);
    Mz(k)=pmr(2,k-1)+vm*dt*cos(thetam)*sin(psim);
    My(k)=pmr(3,k-1)+vm*dt*sin(thetam);
    pmr(:,k)=[Mx(k);Mz(k);My(k)];
    phisp=atan((ptr(3,k)-pmr(3,k))/(ptr(1,k)-pmr(1,k)));
    phicz=atan((ptr(2,k)-pmr(2,k))/(ptr(1,k)-pmr(1,k)));
    dphisp=(phisp-thetam)/dt;
    dphicz=(phicz-psim)/dt;   
    dthetam=m*dphisp;
    dpsim=m*dphicz;   
    thetam=thetam+dthetam*dt;
    psim=psim+dpsim*dt;
    %figure(1);plot(k*dt,thetam*57.3),hold on,
    %figure(2);plot(k*dt,psim*57.3),hold on,
    r(k)=sqrt((ptr(1,k)-pmr(1,k))^2+(ptr(2,k)-pmr(2,k))^2+(ptr(3,k)-pmr(3,k))^2);
    if r(k)<10;break;end;
end;
sprintf('修正比例导引遭遇时间:%3.1f',time),
sprintf('修正比例导引r(k):%3.1f',r(k)),
figure(1);plot3(pmr(1,:),pmr(2,:),pmr(3,:),'k',ptr(1,:),ptr(2,:),ptr(3,:));
%text(x(80),y(80),z(80),'\leftarrow 比例导引');
%axis([0 100 0 100 0 100]);
grid on;