c11l3.m

来自「这是zarchan书的fundamentals of kalman filter」· M 代码 · 共 86 行

SIGNOISE=300.;
X=0.;
Y=0.;
XR1=1000000.;
YR1=20000.*3280.;
XR2=50000000.;
YR2=20000.*3280.;
R1H=0.;
R1DH=0.;
R1DDH=0.;
R2H=0.;
R2DH=0.;
R2DDH=0.;
TS=1.;
TF=100.;
T=0.;
S=0.;
H=.01;
count=0;
XN=0;
while T<=TF 
	XR1OLD=XR1;
	XR2OLD=XR2;
	XR1D=-14600.;
	XR2D=-14600.;
 	XR1=XR1+H*XR1D;
	XR2=XR2+H*XR2D;
 	T=T+H;
	XR1D=-14600.;
	XR2D=-14600.;
 	XR1=.5*(XR1OLD+XR1+H*XR1D);
	XR2=.5*(XR2OLD+XR2+H*XR2D);
 	S=S+H;
	if S>=(TS-.00001)
		S=0.;
		XN=XN+1.;
		K1=3*(3*XN*XN-3*XN+2)/(XN*(XN+1)*(XN+2));
		K2=18*(2*XN-1)/(XN*(XN+1)*(XN+2)*TS);
		K3=60/(XN*(XN+1)*(XN+2)*TS*TS);
		R1NOISE=SIGNOISE*randn;
		R2NOISE=SIGNOISE*randn;
		R1=sqrt((XR1-X)^2+(YR1-Y)^2);
		R2=sqrt((XR2-X)^2+(YR2-Y)^2);
		R1S=R1+R1NOISE;
		R2S=R2+R2NOISE;
		RES1=R1S-R1H-TS*R1DH-.5*TS*TS*R1DDH;
		R1H=R1H+R1DH*TS+.5*R1DDH*TS*TS+K1*RES1;
		R1DH=R1DH+R1DDH*TS+K2*RES1;
		R1DDH=R1DDH+K3*RES1;
		RES2=R2S-R2H-TS*R2DH-.5*TS*TS*R2DDH;
		R2H=R2H+R2DH*TS+.5*R2DDH*TS*TS+K1*RES2;
		R2DH=R2DH+R2DDH*TS+K2*RES2;
		R2DDH=R2DDH+K3*RES2;
		A1=(YR1-YR2)/(XR2-XR1);
		B1=(R1H^2-R2H^2-XR1^2-YR1^2+XR2^2+YR2^2)/(2.*(XR2-XR1));
		A=1.+A1^2;
		B=2.*A1*B1-2.*A1*XR1-2.*YR1;
		C=XR1^2-2.*XR1*B1+YR1^2-R1H^2;
		YH=(-B-sqrt(B^2-4.*A*C))/(2.*A);
		XH=A1*YH+B1;
		TOP=((XR1-X)*(XR2-X)+(YR1-Y)*(YR2-Y));
		BOT=(R1*R2);
		THET=atan2(sqrt(BOT^2-TOP^2),TOP);
		count=count+1;
		ArrayT(count)=T;
		ArrayX(count)=X;
		ArrayXH(count)=XH;
		ArrayY(count)=Y;
		ArrayYH(count)=YH;
		end
end
figure
plot(ArrayT,ArrayX,ArrayT,ArrayXH),grid
xlabel('Time (Sec)')
ylabel('Receiver Downrange (Ft)')
axis([0 100 -900 900])
figure
plot(ArrayT,ArrayY,ArrayT,ArrayYH),grid
xlabel('Time (Sec)')
ylabel('Receiver Altitude (Ft)')
axis([0 100 -1000 1000])
clc
output=[ArrayT',ArrayX',ArrayXH',ArrayY',ArrayYH'];
save datfil.txt output  -ascii
disp 'simulation finished'