c11l8.m

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

TAU=5.;
PHI=9000.;
T=0.;
S=0.;
H=.01;
SIG=sqrt(PHI/H);
X=0.;
Y1=0.;
XDP=100.;
TS=1.;
TF=200.;
count=0;
while T<=TF 
	X1=SIG*randn;
 	XOLD=X;
	Y1OLD=Y1;
	Y1D=(X1-Y1)/TAU;
	XD=XDP+Y1;
	X=X+H*XD;
	Y1=Y1+H*Y1D;
 	T=T+H;
	Y1D=(X1-Y1)/TAU;
	XD=XDP+Y1;
	X=.5*(XOLD+X+H*XD);
	Y1=.5*(Y1OLD+Y1+H*Y1D);
	S=S+H;
	if S>=(TS-.00001)
		S=0.;
		count=count+1;
		ArrayT(count)=T;
		ArrayXD(count)=XD;
		ArrayX(count)=X;
	end
end
figure
plot(ArrayT,ArrayXD),grid
xlabel('Time (Sec)')
ylabel('Receiver Downrange Velocity (Ft/Sec)')
axis([0 200 0 150])
figure
plot(ArrayT,ArrayX),grid
xlabel('Time (Sec)')
ylabel('Receiver Downrange (Ft)')
axis([0 200 0 20000])
clc
output=[ArrayT',ArrayXD',ArrayX'];
save datfil.txt output  -ascii
disp 'simulation finished'