c7l4.m

这是zarchan书的fundamentals of kalman filter的matlab原程序.对学习卡尔曼滤波非常有帮助

X=200000.;
XD=-6000.;
BETA=500.;
XH=X;
XDH=XD;
TS=.1;
TF=30.;
T=0.;
S=0.;
H=.001;
HP=.1;
count=0;
while T<=TF 
	XOLD=X;
	XDOLD=XD;
 	XDD=.0034*32.2*XD*XD*exp(-X/22000.)/(2.*BETA)-32.2;
 	X=X+H*XD;
	XD=XD+H*XDD;
 	T=T+H;
	XDD=.0034*32.2*XD*XD*exp(-X/22000.)/(2.*BETA)-32.2;
 	X=.5*(XOLD+X+H*XD);
	XD=.5*(XDOLD+XD+H*XDD);
 	S=S+H;
	if S>=(TS-.00001)
		S=0.;
		[XB,XDB,XDDB]=PROJECT(T,TS,XH,XDH,BETA,HP);
		XH=XB;
		XDH=XDB;
		ERRX=X-XH;
		ERRXD=XD-XDH;
		count=count+1;
		ArrayT(count)=T;
		ArrayERRX(count)=ERRX;
		ArrayERRXD(count)=ERRXD;
	end
end
figure
plot(ArrayT,ArrayERRX),grid
xlabel('Time (Sec)')
ylabel('Error in Estimate of Altitude (Ft)')
axis([0 30 -30 30])
figure
plot(ArrayT,ArrayERRXD),grid
xlabel('Time (Sec)')
ylabel('Error in Estimate of Velocity (Ft/Sec)')
axis([0 30 -2 12])
clc
output=[ArrayT',ArrayERRX',ArrayERRXD'];
save datfil.txt output  -ascii
disp 'simulation finished'