c7l1.m

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

G=32.2;
X=200000.;
XD=-6000.;
BETA=500.;
TS=.1;
TF=30.;
T=0.;
S=0.;
H=.001;
count=0;
while T<=TF 
	XOLD=X;
	XDOLD=XD;
	XDD=.0034*G*XD*XD*exp(-X/22000.)/(2.*BETA)-G;
 	X=X+H*XD;
	XD=XD+H*XDD;
 	T=T+H;
	XDD=.0034*G*XD*XD*exp(-X/22000.)/(2.*BETA)-G;
 	X=.5*(XOLD+X+H*XD);
	D=.5*(XDOLD+XD+H*XDD);
 	S=S+H;
	if S>=(TS-.00001)
		S=0.;
		count=count+1;
		ArrayT(count)=T;
		ArrayX(count)=X;
		ArrayXD(count)=XD;
		ArrayXDD(count)=XDD;
	end
end
figure
plot(ArrayT,ArrayX),grid
xlabel('Time (Sec)')
ylabel('Altitude (Ft)')
axis([0 30 0 200000])
figure
plot(ArrayT,ArrayXD),grid
xlabel('Time (Sec)')
ylabel('Velocity (Ft/Sec)')
axis([0 30 -7000 -3000])
figure
plot(ArrayT,ArrayXDD),grid
xlabel('Time (Sec)')
ylabel('Acceleration (Ft/Sec^2)')
axis([0 30 -100 400])
clc
output=[ArrayT',ArrayX',ArrayXD',ArrayXDD'];
save datfil.txt output  -ascii
disp 'simulation finished'