三维比例导引MATLAB程序

比例导引三维仿真轨迹 ，matlab程序 ，仿真效果不错，对弹道仿真很有帮助

%给出了比例导引法的差分方程 采用Matlab语言 对比例导引法的导弹理想弹道进行了三维数据仿真 绘制出三维理想弹道。随导弹与目标参数变化情况 对理想弹道的特性进行了分析
clear all
clc
clear
tt=0.1;
sm=0.6*tt;
st=0.42*tt;
x（1）=0;y（1）=0;z（1）=0;
pmr（:1）=[x（1）;y（1）;z（1）];%导弹起始位置
ptr（:1）=[25;5;10];%目标起始位置
m=3;%比例导引系数
q（1）=0;
o（1）=0;
a（1）=0;
for（k=2:600）
ptr（:k）=[25-0.42*cos（pi/6）*tt*k;5;10+0.42*sin（pi/6）*k*tt];
r（k-1）=sqrt（（ptr（1k-1）-pmr（1k-1））^2+（ptr（2k-1）-pmr（2k-1））^2+（ptr（3k-1）-pmr（3k-1））^2）; %目标和导弹相对距离
c=sqrt（（ptr（1k）-pmr（1k-1））^2+（ptr（2k）-pmr（2k-1））^2+（ptr（3k）-pmr（3k-1））^2）;
b=acos（（r（k-1）^2+st^2-c^2）/（2*r（k-1）*st））;
dq=acos（（r（k-1）^2-st^2+c^2）/（2*r（k-1）*c））;
if abs（imag（b））>0
b=0.0000001;
end 
if abs（imag（dq））>0
dq=0.0000001;
end 
q（k）=q（k-1）+dq;
o（k）=o（k-1）+m*dq;
a（k）=o（k）-q（k）;
c1=r（k-1）*sin（b）/sin（a（k）+b）;
c2=r（k-1）*sin（a（k））/sin（a（k）+b）;
c3=sqrt（（c1-sm）^2+（c2-st）^2+2*（c1-sm）*（c2-st）*cos（a（k）+b））;
dq=a（k）-acos（（（c1-sm）^2+c3^2-（c2-st）^2）/（2*（c1-sm）*c3））;
if abs（imag（dq））>0
dq=0.0000001;
end 
q（k）=q（k-1）+dq;
o（k）=o（k-1）+m*dq;
a（k）=o（k）-q（k）;
c1=r（k-1）*sin（b）/sin（a（k）+b）;
c2=r（k-1）*sin（a（k））/sin（a（k）+b）;
c3=sqrt（（c1-sm）^2+（c2-st）^2+2*（c1-sm）*（c2-st）*cos（a（k）+b））;
dq=a（k）-acos（（（c1-sm）^2+c3^2-（c2-st）^2）/（2*（c1-sm）*c3））;
if abs（imag（dq））>0
dq=0.0000001;
end
q（k）=q（k-1）+dq;
o（k）=o（k-1）+m*dq;
a（k）=o（k）-q（k）;
c1=r（k-1）*sin（b）/sin（a（k）+b）;
c2=r（k-1）*sin（a（k））/sin（a（k）+b）;
c3=sqrt（（c1-sm）^2+（c2-st）^2+2*（c1-sm）*（c2-st）*cos（a（k）+b））;
x1（k）=ptr（1k-1）+c2/st*（ptr（1k）-ptr（1k-1））;
y1（k）=ptr（2k-1）+c2/st*（ptr（2k）-ptr（2k-1））;
z1（k）=ptr（3k-1）+c2/st*（ptr（3k）-ptr（3k-1））;
x（k）=pmr（1k-1）+sm/c1*（x1（k）-pmr（1k-1））;
y（k）=pmr（2k-1）+sm/c1*（y1（k）-pmr（2k-1））;
z（k）=pmr（3k-1）+sm/c1*（z1（k）-pmr（3k-1））;
pmr（:k）=[x（k）;y（k）;z（k）];
r（k）=sqrt（（ptr（1k）-pmr（1k））^2+（ptr（2k）-pmr（2k））^2+（ptr（3k）-pmr（3k））^2）;
if r（k）<0.06;
break;
end;
end
sprintf（‘遭遇时间：%3.1f‘0.1*k）;
figure（1）;
plot3（pmr（11:k）pmr（21:k）pmr（31:k）‘g‘ptr（1:）ptr（2:）ptr（3:）‘r‘）;
axis（[0 25 0 5 0 25]）;
text（x（80）y（80）z（80）‘\leftarrow 比例导引‘）;
grid on

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       2152  2011-06-24 17:37  bilidaoyin.m

----------- ---------  ---------- -----  ----

                 2152                    1