clear?all


n=0;
T=0.;
S=0.;
H=.002;

VM?=?3500.;
VT?=?7300.;


k?=?3;?????????%導航比???N
AT?=?5.;????%目標加速度nt???????????最終要算的是NC???然后刷新導彈的加速度
drt?=?-8558;??????????%VC??r（t）一階導數


HE=0;???????%轉化為弧度角?航向誤差??HE
SEITA?=?-1.5/57.3;??????%初始λ?50°
BETA?=?0./57.3;


RM1?=?0.;
RM2?=?0.;
RT1?=?100000.;
RT2?=?1000.;

RTM1=RT1-RM1;????????%相對位置
RTM2=RT2-RM2;
rt=sqrt（RTM1*RTM1+RTM2*RTM2）;???%rtm??
ry3t?=?RT2-RM2;



qt?=?ry3t/rt;

VM1?=?VM*cos（SEITA）;?????%刷新導彈速度
VM2?=?VM*sin（SEITA）;
VT1?=?-VT*cos（BETA）;
VT2?=?VT*sin（BETA）;
VTM1?=?VT1?-?VM1;??????????%相對速度大小
VTM2?=?VT2?-?VM2;

SEITA?=?atan2（RTM2RTM1）;%tan反函數
SEITAD=（RTM1*VTM2-RTM2*VTM1）/rt^2;

dry3t?=?VT2?-?VM2;???????????????????%?核心算法
dqt?=?（dry3t*rt-ry3t*drt）/（rt*rt）;
u?=?（k+1）*drt*dqt+hardlim（dqt）;


AM1=u*sin（SEITA）;????????????%導彈加速度分量???qt?仰角
AM2=-u*cos（SEITA）;?


while?drt?<=?0??????%只要接近速度>0??就得繼續
?????
????BETAOLD=BETA;????????%存數據??????
????RT1OLD=RT1;
????RT2OLD=RT2;
????RM1OLD=RM1;
????RM2OLD=RM2;
????VM1OLD=VM1;
????VM2OLD=VM2;
????STEP=1;
????FLAG=0;
????while?STEP?<=1?
????????if?FLAG==1
????????????STEP=2;?
????????????BETA=BETA+H*BETAD;??????%β=β+H*β的一階導數
????????????RT1=RT1+H*VT1;??????????%根據步長改變數據
????????????RT2=RT2+H*VT2;
????????????RM1=RM1+H*VM1;
????????????RM2=RM2+H*VM2;?
????????????VM1=VM1+H*AM1;?
????????????VM2=VM2+H*AM2;
????????????T=T+H;?????????????????%T是步長H的累計
????????end
????????
????????RTM1=RT1-RM1;
????????RTM2=RT2-RM2;
????????SEITA=atan2（RTM2RTM1）;
????????ry3t?=?RT2-RM2;
????????rt=sqrt（RTM1*RTM1+RTM2*RTM2）;???

????????VTM1?=?VT1?-?VM1;??????????%相對速度大小
????????VTM2?=?VT2?-?VM2;
????????
????????SEITAD=（RTM1*VTM2-RTM2*VTM1）/rt^2;
????????
????????VM?=?sqrt（VM1*VM1+VM2*VM2）;???
????????
????????drt?=?（RTM1*VTM1+RTM2*VTM2）/rt;
????????qt?=?ry3t/rt;
????????dry3t?=?VT2?-?VM2;
????????dqt?=?（dry3t*rt-ry3t*drt）/（rt*rt）;
????????u?=?（k+1）*drt*dqt+hardlim（dqt）;
????????AM1=u*sin（SEITA）;????????????%導彈加速度分量???qt?仰角
????????AM2=-u*cos（SEITA）;?
????????
????????VT1=-VT*cos（BETA）;
????????VT2=-VT*sin（BETA）;
????????BETAD=AT/VT;??????????%β的一階導數
????????FLAG=1;
????end
????FLAG=0;?
????BETA=.5*（BETAOLD+BETA+H*BETAD）;???%老數據加上新數據?再?乘1/2
????RT1=.5*（RT1OLD+RT1+H*VT1）;?
????RT2=.5*（RT2OLD+RT2+H*VT2）;
????RM1=.5*（RM1OLD+RM1+H*VM1）;
????RM2=.5*（RM2OLD+RM2+H*VM2）;
????VM1=.5*（VM1OLD+VM1+H*AM1）;?
????VM2=.5*（VM2OLD+VM2+H*AM2）;?
????S=S+H;
????if?S?>=.09999??????????????????%計算這么多次后??存一下值??大概10次
????????S=0.;
????????n=n+1;
????????ArrayT（n）=T;
????????ArrayRT1（n）=RT1;
????????ArrayRT2（n）=RT2;
????????ArrayRM1（n）=RM1;
????????ArrayRM2（n）=RM2;
????????ArrayXNCG（n）=u;
????????ArrayRTM（n）=rt;
????????ArrayQT（n）=qt;
????????ArraySEITAD（n）=SEITAD;
????????ArrayK（n）=-drt/rt;
????end
????
end
figure
plot（ArrayRT1ArrayRT2ArrayRM1ArrayRM2）grid
title（‘Eventual?ight?path?in?the?xy?plane‘）?
xlabel（‘X?‘）
ylabel（‘Z‘）
figure
plot（ArrayTArraySEITAD）grid
titl

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

?????文件???????3442??2020-07-06?16:49??ASMG\asmg_2.m

?????目錄??????????0??2020-08-06?11:03??ASMG

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

?????????????????3442????????????????????2