c13l1.m

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

X=200000.;
XD=-6000.;
BETA=500.;
XNOM=200000.;
XDNOM=-6000.;
BETANOM=800.;
TS=.1;
T=0.;
S=0.;
H=.001;
count=0;
while X>=0.
	XOLD=X;
	XDOLD=XD;
	XNOMOLD=XNOM;
	XDNOMOLD=XDNOM;
	XDD=.0034*32.2*XD*XD*exp(-X/22000.)/(2.*BETA)-32.2;
	XDDNOM=.0034*32.2*XDNOM*XDNOM*exp(-XNOM/22000.)/(2.*BETANOM)-32.2;
 	X=X+H*XD;
	XD=XD+H*XDD;
	XNOM=XNOM+H*XDNOM;
	XDNOM=XDNOM+H*XDDNOM;
 	T=T+H;
	XDD=.0034*32.2*XD*XD*exp(-X/22000.)/(2.*BETA)-32.2;
	XDDNOM=.0034*32.2*XDNOM*XDNOM*exp(-XNOM/22000.)/(2.*BETANOM)-32.2;
 	X=.5*(XOLD+X+H*XD);
	XD=.5*(XDOLD+XD+H*XDD);
	XNOM=.5*(XNOMOLD+XNOM+H*XDNOM);
	XDNOM=.5*(XDNOMOLD+XDNOM+H*XDDNOM);
 	S=S+H;
	if S>=(TS-.00001)
		S=0.;
		DELXH=X-XNOM;
		DELXDH=XD-XDNOM;
		DELBETAH=BETA-BETANOM;
		XH=XNOM+DELXH;
		XDH=XDNOM+DELXDH;
		BETAH=BETANOM+DELBETAH;
		count=count+1;
		ArrayT(count)=T;
		ArrayX(count)=X;
		ArrayXD(count)=XD;
		ArrayDELXH(count)=DELXH;
		ArrayDELXDH(count)=DELXDH;
		end
end
figure
plot(ArrayT,ArrayX),grid
xlabel('Time (Sec)')
ylabel('Altitude (Ft)')
axis([0 50 0 200000])
figure
plot(ArrayT,ArrayXD),grid
xlabel('Time (Sec)')
ylabel('Velocity (Ft/Sec)')
axis([0 50 -7000 0])
figure
plot(ArrayT,ArrayDELXH),grid
xlabel('Time (Sec)')
ylabel('Error in Altitude (Ft)')
axis([0 50 0 15000])
figure
plot(ArrayT,ArrayDELXDH),grid
xlabel('Time (Sec)')
ylabel('Error in Velocity (Ft/Sec)')
axis([0 50 0 1000])
clc
output=[ArrayT',ArrayX',ArrayXD',ArrayDELXH',ArrayDELXDH'];
save datfil.txt output  -ascii
disp 'simulation finished'