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;endXF=X;YF=Y;