predict1.m

来自「这是本战略战术导弹制导的书中的matlab程序,比书中的 forchan程序简单」· M 代码 · 共 52 行

function[XF,YF]=PREDICT1(TP,XP,YP,XDP,YDP,THETP,TF,THADP,THBDP,ACC,HP,THDMAX,ACCERR)
H=HP*10.;
T=TP;
X=XP;
Y=YP;
XD=XDP;
YD=YDP;
THET=THETP;
THAD=THADP;
THBD=THBDP;
S=0.;
while T<=(TF-.00001)
	XOLD=X;
	YOLD=Y;
	XDOLD=XD;
	YDOLD=YD;
	THETOLD=THET;
	STEP=1;
	FLAG=0;
	while STEP <=1
		if FLAG==1
         		STEP=2;
			X=X+H*XD;
			Y=Y+H*YD;
			XD=XD+H*XDD;
			YD=YD+H*YDD;
			THET=THET+H*THETD;
			T=T+H;
		end
		SLOPE=(THBD-THAD)/TF;
		BINT=THBD-SLOPE*TF;
		THETD=SLOPE*T+BINT;
		if THETD>THDMAX
			THETD=THDMAX;
		end
		if THETD<-THDMAX
			THETD=-THDMAX;
		end
		XDD=(ACC+ACCERR)*cos(THET);
		YDD=(ACC+ACCERR)*sin(THET);
		FLAG=1;
	end
	FLAG=0;
	X=(XOLD+X)/2+.5*H*XD;
	Y=(YOLD+Y)/2+.5*H*YD;
	XD=(XDOLD+XD)/2+.5*H*XDD;
	YD=(YDOLD+YD)/2+.5*H*YDD;
	THET=(THETOLD+THET)/2.+.5*H*THETD;
end
XF=X;
YF=Y;