c3l2.m

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

TS=.1;
SIGNOISE=1.;
for K=1:5
	A0=1.;
	A1=0.;
	XH=0.;
	XN=0;
	count=0;
	arrayt  = [0:TS:10];
	for T=0:TS:10
		XN=XN+1.;
		XNOISE=SIGNOISE*randn;
		ACT=A0+A1*T;
		XS=ACT+XNOISE;
		XK=1./XN;
		RES=XS-XH;
		XH=XH+XK*RES;
		SP11=SIGNOISE/sqrt(XN);
		SP11P=-SP11;
		XHERR=ACT-XH;
		EPS=.5*A1*TS*(XN-1);
		count=count+1;
   		ArrayXHERR(K,count)=XHERR;
   		ArraySP11(K,count)=SP11;
   		ArraySP11P(K,count)=SP11P;
 	end
end
figure
plot(arrayt,ArrayXHERR,arrayt,ArraySP11,arrayt,ArraySP11P),grid
xlabel('Time (Sec)')
ylabel('Error in Estimate of x')
axis([0 10 -1 1])
clc
disp 'simulation finished'