c16l2.m

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

count=0;
A=2.0926e7;
GM=1.4077e16;
XNP=3.;
TS=.1;
XLONGTDEG=90.;
XLONGMDEG=85.;
DEGRAD=57.3;
SIGLAM=.001;
PREDERR=0.;
XISP1=250.;
XISP2=250.;
XMF1=.85;
XMF2=.85;
WPAY=100.;
DELV=20000.;
DELV1=.3333*DELV;
DELV2=.6667*DELV;
AMAX1=20.;
AMAX2=20.;
XKICKDEG=80.;
TOP2=WPAY*(exp(DELV2/(XISP2*32.2))-1.);
BOT2=1/XMF2-((1.-XMF2)/XMF2)*exp(DELV2/(XISP2*32.2));
WP2=TOP2/BOT2;
WS2=WP2*(1-XMF2)/XMF2;
WTOT2=WP2+WS2+WPAY;
TRST2=AMAX2*(WPAY+WS2);
TB2=XISP2*WP2/TRST2;
TOP1=WTOT2*(exp(DELV1/(XISP1*32.2))-1.);
BOT1=1/XMF1-((1.-XMF1)/XMF1)*exp(DELV1/(XISP1*32.2));
WP1=TOP1/BOT1;
WS1=WP1*(1-XMF1)/XMF1;
WTOT=WP1+WS1+WTOT2;
TRST1=AMAX1*(WTOT2+WS1);
TB1=XISP1*WP1/TRST1;
ALTNMT=0.;
ALTNMM=0.;
ALTT=ALTNMT*6076.;
ALTM=ALTNMM*6076.;
S=0.;
XLONGM=XLONGMDEG/DEGRAD;
XLONGT=XLONGTDEG/DEGRAD;
XM=A*cos(XLONGM);
YM=A*sin(XLONGM);
XT=(A+ALTT)*cos(XLONGT);
YT=(A+ALTT)*sin(XLONGT);
XFIRSTT=XT;
YFIRSTT=YT;
X1T=cos(XKICKDEG/57.3);
Y1T=sin(XKICKDEG/57.3);
K1=0.;
K2=0.;
K3=0.;
K1P=0.;
K2P=0.;
K3P=0.;
XTH=XT;
XTDH=X1T;
XTDDH=0.;
YTH=YT;
YTDH=Y1T;
YTDDH=0.;
T=0.;
TF=50.;
PHIN=100*100/TF;
[XTF,YTF]=predictb(TF,XT,YT,X1T,Y1T,WP1,WTOT,TB1,TRST1,TB2,WP2,WTOT2,TRST2,WPAY);
YTF=YTF+PREDERR;
[VRXM,VRYM]=lambert(XM,YM,TF,XTF,YTF,XLONGM,XLONGT);
X1M=VRXM;
Y1M=VRYM;
RTM1=XT-XM;
RTM2=YT-YM;
RTM=sqrt(RTM1^2+RTM2^2);
XLAM=atan2(RTM2,RTM1);
SIGX2=(SIGLAM*RTM*sin(XLAM))^2;
SIGX=sqrt(SIGX2);
P11=SIGX2;
P12=0.;
P13=0.;
P22=0.;
P23=0.;
P33=100*100;
SIGY2=(SIGLAM*RTM*cos(XLAM))^2;
SIGY=sqrt(SIGY2);
P11P=SIGY2;
P12P=0.;
P13P=0.;
P22P=0.;
P23P=0.;
P33P=100*100;
VTM1=X1T-X1M;
VTM2=Y1T-Y1M;
VC=-(RTM1*VTM1+RTM2*VTM2)/RTM;
DELV=0.;
while VC>=0.
 	TGO=RTM/VC;
 	if TGO>.1
		H=.01;
	else
		H=.0001;
	end
	XOLDT=XT;
	YOLDT=YT;
	X1OLDT=X1T;
	Y1OLDT=Y1T;
	XOLDM=XM;
	YOLDM=YM;
	X1OLDM=X1M;
	Y1OLDM=Y1M;
	DELVOLD=DELV;
	STEP=1;
	FLAG=0;
	while STEP <=1
		if FLAG==1
			XT=XT+H*XDT;
			YT=YT+H*YDT;
			X1T=X1T+H*X1DT;
			Y1T=Y1T+H*Y1DT;
			XM=XM+H*XDM;
			YM=YM+H*YDM;
			X1M=X1M+H*X1DM;
			Y1M=Y1M+H*Y1DM;
			DELV=DELV+H*DELVD;
			T=T+H;
			STEP=2;
		end
		if T<TB1
			WGT=-WP1*T/TB1+WTOT;
			TRST=TRST1;
		elseif T<(TB1+TB2)
			WGT=-WP2*T/TB2+WTOT2+WP2*TB1/TB2;
			TRST=TRST2;
		else
			WGT=WPAY;
			TRST=0.;
		end
		AT=32.2*TRST/WGT;
		VEL=sqrt(X1T^2+Y1T^2);
		AXT=AT*X1T/VEL;
		AYT=AT*Y1T/VEL;
		TEMBOTT=(XT^2+YT^2)^1.5;
		X1DT=-GM*XT/TEMBOTT+AXT;
		Y1DT=-GM*YT/TEMBOTT+AYT;
		XDT=X1T;
		YDT=Y1T;
		RTM1=XT-XM;
		RTM2=YT-YM;
		RTM=sqrt(RTM1^2+RTM2^2);
		VTM1=X1T-X1M;
		VTM2=Y1T-Y1M;
		VC=-(RTM1*VTM1+RTM2*VTM2)/RTM;
		TGO=RTM/VC;
		XLAM=atan2(RTM2,RTM1);
		XLAMD=(RTM1*VTM2-RTM2*VTM1)/(RTM*RTM);
		ATPLOS=Y1DT*cos(XLAM)-X1DT*sin(XLAM);
		XNC=XNP*VC*XLAMD+.5*XNP*ATPLOS;
		DELVD=abs(XNC);
		AM1=-XNC*sin(XLAM);
		AM2=XNC*cos(XLAM);
		TEMBOTM=(XM^2+YM^2)^1.5;
		X1DM=-GM*XM/TEMBOTM+AM1;
		Y1DM=-GM*YM/TEMBOTM+AM2;
		XDM=X1M;
		YDM=Y1M;
		FLAG=1;
	end;
	FLAG=0;
 	XT=(XOLDT+XT)/2+.5*H*XDT;
	YT=(YOLDT+YT)/2+.5*H*YDT;
	X1T=(X1OLDT+X1T)/2+.5*H*X1DT;
	Y1T=(Y1OLDT+Y1T)/2+.5*H*Y1DT;
	XM=(XOLDM+XM)/2+.5*H*XDM;
	YM=(YOLDM+YM)/2+.5*H*YDM;
	X1M=(X1OLDM+X1M)/2+.5*H*X1DM;
	Y1M=(Y1OLDM+Y1M)/2+.5*H*Y1DM;
	DELV=(DELVOLD+DELV)/2.+.5*H*DELVD;
	ALTT=sqrt(XT^2+YT^2)-A;
	ALTM=sqrt(XM^2+YM^2)-A;
	S=S+H;
	if S>=(TS-.00001)
 		S=0.;
		TS2=TS*TS;
		TS3=TS2*TS;
		TS4=TS3*TS;
		TS5=TS4*TS;
		SIGX2=(SIGLAM*RTM*sin(XLAM))^2;
		SIGY2=(SIGLAM*RTM*cos(XLAM))^2;
		SIGX=sqrt(SIGX2);
		SIGY=sqrt(SIGY2);
		M11=P11+TS*P12+.5*TS2*P13+TS*(P12+TS*P22+.5*TS2*P23);
		M11=M11+.5*TS2*(P13+TS*P23+.5*TS2*P33)+TS5*PHIN/20.;
		M12=P12+TS*P22+.5*TS2*P23+TS*(P13+TS*P23+.5*TS2*P33)+TS4*PHIN/8.;
		M13=P13+TS*P23+.5*TS2*P33+PHIN*TS3/6.;
		M22=P22+TS*P23+TS*(P23+TS*P33)+PHIN*TS3/3.;
		M23=P23+TS*P33+.5*TS2*PHIN;
		M33=P33+PHIN*TS;
		BOT=M11+SIGX2;
		K1=M11/BOT;
		K2=M12/BOT;
		K3=M13/BOT;
		FACT=1.-K1;
		P11=FACT*M11;
		P12=FACT*M12;
		P13=FACT*M13;
		P22=-K2*M12+M22;
		P23=-K2*M13+M23;
		P33=-K3*M13+M33;
		M11P=P11P+TS*P12P+.5*TS2*P13P+TS*(P12P+TS*P22P+.5*TS2*P23P);
		M11P=M11P+.5*TS2*(P13P+TS*P23P+.5*TS2*P33P)+TS5*PHIN/20.;
		M12P=P12P+TS*P22P+.5*TS2*P23P+TS*(P13P+TS*P23P+.5*TS2*P33P)+TS4*PHIN/8.;
		M13P=P13P+TS*P23P+.5*TS2*P33P+PHIN*TS3/6.;
		M22P=P22P+TS*P23P+TS*(P23P+TS*P33P)+PHIN*TS3/3.;
		M23P=P23P+TS*P33P+.5*TS2*PHIN;
		M33P=P33P+PHIN*TS;
		BOTP=M11P+SIGY2;
		K1P=M11P/BOTP;
		K2P=M12P/BOTP;
		K3P=M13P/BOTP;
		FACTP=1.-K1P;
		P11P=FACTP*M11P;
		P12P=FACTP*M12P;
		P13P=FACTP*M13P;
		P22P=-K2P*M12P+M22P;
		P23P=-K2P*M13P+M23P;
		P33P=-K3P*M13P+M33P;
		XLAMNOISE=gaussc7(SIGLAM);
		YTMEAS=YM+RTM*sin(XLAM+XLAMNOISE);
		XTMEAS=XM+RTM*cos(XLAM+XLAMNOISE);
		XNOISE=XT-XTMEAS;
		YNOISE=YT-YTMEAS;
		RESX=XTMEAS-XTH-TS*XTDH-.5*TS2*XTDDH;
		XTH=K1*RESX+XTH+TS*XTDH+.5*TS2*XTDDH;
		XTDH=K2*RESX+XTDH+TS*XTDDH;
		XTDDH=K3*RESX+XTDDH;
		RESY=YTMEAS-YTH-TS*YTDH-.5*TS2*YTDDH;
		YTH=K1P*RESY+YTH+TS*YTDH+.5*TS2*YTDDH;
		YTDH=K2P*RESY+YTDH+TS*YTDDH;
		YTDDH=K3P*RESY+YTDDH;
		X1DTG=X1DT/32.2;
		XTDDHG=XTDDH/32.2;
		Y1DTG=Y1DT/32.2;
		YTDDHG=YTDDH/32.2;
		count=count+1;
		ArrayT(count)=T;
		ArrayXNOISE(count)=XNOISE;
		ArraySIGX(count)=SIGX;
		ArrayYNOISE(count)=YNOISE;
		ArraySIGY(count)=SIGY;
		ArrayX1DTG(count)=X1DTG;
		ArrayXTDDHG(count)=XTDDHG;
		ArrayY1DTG(count)=Y1DTG;
		ArrayYTDDHG(count)=YTDDHG;
	end
end
RTM
DELV	
figure
plot(ArrayT,ArrayXNOISE,ArrayT,ArraySIGX,ArrayT,-ArraySIGX),grid
xlabel('Time (Sec)')
ylabel('XT Noise (Ft) ')
figure
plot(ArrayT,ArrayYNOISE,ArrayT,ArraySIGY,ArrayT,-ArraySIGY),grid
xlabel('Time (Sec)')
ylabel('YT Noise (Ft) ')
figure
plot(ArrayT,ArrayX1DTG,ArrayT,ArrayXTDDHG),grid
xlabel('Time (Sec)')
ylabel('Acceleration Along XT (G) ')
figure
plot(ArrayT,ArrayY1DTG,ArrayT,ArrayYTDDHG),grid
xlabel('Time (Sec)')
ylabel('Acceleration Along YT (G) ')
clc
output=[ArrayT',ArrayXNOISE',ArraySIGX',-ArraySIGX',ArrayYNOISE',ArraySIGY',-ArraySIGY',ArrayX1DTG',ArrayXTDDHG',ArrayY1DTG',ArrayYTDDHG'];
save datfil.txt output /ascii
disp 'simulation finished'