%反空地導(dǎo)彈目標(biāo)比例導(dǎo)引三維彈道仿真
%紅色段為自由落體段
%藍(lán)色段為自主控制段
%黑色段為防空導(dǎo)彈彈道

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;%導(dǎo)彈投放高度
hi=4000;%導(dǎo)彈由自由落體轉(zhuǎn)自主控制時(shí)的高度    
h1=3000;%預(yù)定平飛高度
vx0=v0;
xt0=20000;

t1=sqrt(2*(h0-hi)/g);      %啟控點(diǎn)時(shí)間


thetat1=atan(-sqrt(2*g*(h0-hi))/v0);   %啟控點(diǎn)傾角
x11=xt0+v0*t1;                              %啟控點(diǎn)橫坐標(biāo)
y11=h0-0.5*g*t1^2;                       %啟控點(diǎn)縱坐標(biāo) ，高度


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,  %自由段坐標(biāo)變化
%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              %變步長(zhǎng)
      % 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('修正比例導(dǎo)引遭遇時(shí)間:%3.1f',time),
sprintf('修正比例導(dǎo)引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 比例導(dǎo)引');
%axis([0 100 0 100 0 100]);
grid on;