c5l4.m

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

WN=6.28*.1;
W=6.28*1.;
Z=.7;
A2=.1;
X1=.25;
B2=1.25;
X1D=0.;
B1=1.;
T=0.;
S=0.;
H=.001;
count=0;
while T<=20
	S=S+H;
	X1OLD=X1;
	X1DOLD=X1D;
	X2=A2*sin(W*T);
	X2D=A2*W*cos(W*T);
	X2DD=-A2*W*W*sin(W*T);
	X1DD=-2.*Z*WN*X1D-WN*WN*X1-X2DD;
	X1=X1+H*X1D;
	X1D=X1D+H*X1DD;
	T=T+H;
	X2=A2*sin(W*T);
	X2D=A2*W*cos(W*T);
	X2DD=-A2*W*W*sin(W*T);
	X1DD=-2.*Z*WN*X1D-WN*WN*X1-X2DD;
	X1=.5*(X1OLD+X1+H*X1D);
	X1D=.5*(X1DOLD+X1D+H*X1DD);
	if S>=.09999
		S=0.;
		DIST=X1+X2+B1+B2;
		SUSP=X1+B2;
		count=count+1;
      		ArrayT(count)=T;
      		ArraySUSP(count)=SUSP;
      		ArrayX2(count)=X2;
      		ArrayDIST(count)=DIST;
	end
end
figure
plot(ArrayT,ArraySUSP),grid
xlabel('Time (Sec)')
ylabel('Suspension Length (Ft)')
axis([0 20 0 2])
figure
plot(ArrayT,ArrayX2,ArrayT,ArrayDIST),grid
xlabel('Time (Sec)')
ylabel('X2 and DIST (Ft)')
clc
output=[ArrayT',ArraySUSP',ArrayX2'ArrayDIST'];
save datfil.txt output  -ascii
disp 'simulation finished'