%最優(yōu)制導(dǎo)率仿真初始化參數(shù)+10*pi*rand（1）/180?坐標(biāo)進(jìn)行了修改
%最優(yōu)制導(dǎo)律反TBM彈道仿真
clear?all;
clear;
global?signvc;
g=9.81;
pi=3.14159265;
RHO=1.225;
SONIC=343.13;
dt=0.1;
v0=7*SONIC;
h0=40000;?????????????????????????%關(guān)機(jī)點(diǎn)高度??????????
xt0=652000;
yt0=h0;
h1=80000;????????????????????????????%再入段高度
ammax=15*g;
ae=pi/180;
thetat0=135*ae;???????????????????????%初始彈道傾角
vx0=v0*cos（thetat0）;
vy0=v0*sin（thetat0）;
t=（vy0+sqrt（vy0^2-2*g*（h1-h0）））/g;???%再入段時(shí)間
xt1=xt0+vx0*t;
yt1=h0+vy0*t-0.5*g*t^2;
vx1=vx0;
vy1=vy0-g*t;
v1=sqrt（vx1^2+vy1^2）;

thetat1=atan（（v0*sin（thetat0）-g*t）/（v0*cos（thetat0）））;%再入角

T=0:dt:t;
x1=xt0+vx0*T;
y1=h0+vy0*T-0.5*g*T.^2;
thetatz=atan（（v0*sin（thetat0）-g*T）/（v0*cos（thetat0）））;
%figure（1）;plot（x1y1）hold?on??????????????????????????????????%自由段坐標(biāo)變化
%figure（2）;plot（Tsqrt（vx0.^2+（vy0-g*T）.^2））hold?on????????????%自由段速度變化
%figure（3）;plot（Tthetatz）hold?on

R=6371000;
m=100;
CX=5;
d=0.1698;
i=0.5;
ae=pi/180;
			??????????
thetat=abs（thetat1）;???????????????????????%初始彈道傾角
xt=xt1;
yt=h1;
vt=v1;???????????????????????????????????%初速
ptr（:1）=[xt;yt];

vm=2000;%導(dǎo)彈速度
headerror=0*pi/180;?%指向角誤差

rmx=20000;rmy=0;
pmr（:1）=[rmx;rmy];


vty=-vt*sin（thetat）;%目標(biāo)的速度分量
vtx=-vt*cos（thetat）;
rtmx=xt-rmx;%初始彈目相對(duì)距離
rtmy=yt-rmy;
rtm=sqrt（rtmx^2+rtmy^2）;

sightangle=atan（rtmy/rtmx）;%視線角
leadangle=asin（vt*sin（sightangle-thetat）/vm）;%指向角
vmy=vm*sin（sightangle-leadangle+headerror）;%導(dǎo)彈的速度分量
vmx=vm*cos（sightangle-leadangle+headerror）;
vtmx=vtx-vmx;
vtmy=vty-vmy;
%彈目的相對(duì)運(yùn)動(dòng)速度
vc=-（rtmx*vtmx+rtmy*vtmy）/rtm;
signvc=sign（vc）;?%vc的符號(hào)

time=0;dt=0.1;%時(shí)間和時(shí)間步長(zhǎng)
%pvr（:1）=[time;dtheta];
%file=fopen（‘output.dat‘‘wt‘）;%將數(shù)據(jù)寫(xiě)入文件
%循環(huán)
for?k=2:4000;

????if（rtm<1000）%彈目距離小于100，步長(zhǎng)變?yōu)?.005
???????dt=0.001;
????end


?F=4.75*0.0001*vt^2*CX;
if?yt>=9300
????H=exp（-0.0001*yt）;
end
if?yt<9300
???H=（1-2.1905*10^（-5*yt））^4.4;
end

XF=i*d^2*1000*H*F./g;
dvt=g*sin（thetat）-XF/m;
dthetat=-g*cos（thetat）/vt+vt*cos（thetat）/（R+yt）;
dyt=-vt*sin（thetat）;
dxt=R*vt*cos（thetat）/（R+yt）;

vt=vt+dt*dvt;
thetat=thetat-dt*dthetat;
yt=yt+dt*dyt;
xt=xt-dt*dxt;
at=dthetat*vt;
ptr（:k）=[xt;yt];

vty=-vt*sin（thetat）;%目標(biāo)的速度分量
vtx=-vt*cos（thetat）;
rtmx=xt-rmx;%初始彈目相對(duì)距離
rtmy=yt-rmy;
rtm=sqrt（rtmx^2+rtmy^2）;

%視線角速率
dsightangle=（rtmx*vtmy-rtmy*vtmx）/（rtm^2）;
%dtheta=3*vc*dsightangle/vm;%最優(yōu)比例導(dǎo)引
dtheta=3*dsightangle;

%k1=1.45;k2=1.5;k3=0.3;k5=3;
%dtheta=k1*vc*dsightangle/（vm*（k2+k3*tgo））;

the=thetat+pi;?????????%對(duì)thetat進(jìn)行修正
%dtheta=3*（vc*dsightangle+0.5*g*cos（the））/vm;???????%目標(biāo)加速度修正項(xiàng)??%+不如—好！
theta=atan（vmy/vmx）;
%導(dǎo)彈加速度
am=vm*dtheta;
%限制機(jī)動(dòng)能力
if（abs（am）>ammax）
????am=sign（am）*ammax;
end
%加速度分量
amy=am*cos（sightangle）;
amx=-am*sin（sightangle）;
amfa=am*cos（sightangle-theta）;
time=time+dt;

%導(dǎo)彈速度
vmx=vmx+dt*amx;
vmy=vmy+dt*amy;
vm=sqrt（vmx^2+vmy^2）;
%導(dǎo)彈位置
rmx=rmx+dt*vmx;
rmy=rmy+dt*vmy;
rm=sqrt（rmx^2+rmy^2）;
pmr（:k）=[rmx;rmy];
??pvr（:k）=[time;amfa/g];??
%彈目相對(duì)位移
rtmx=xt-rmx;
rtmy=yt-rmy;

%

?屬性????????????大小?????日期????時(shí)間???名稱
-----------?---------??----------?-----??----

?????文件???????4044??2008-11-05?17:27??最優(yōu)制導(dǎo)律反TBM彈道仿真\zytbm.m

?????目錄??????????0??2008-11-05?17:27??最優(yōu)制導(dǎo)律反TBM彈道仿真

-----------?---------??----------?-----??----

?????????????????4044????????????????????2