c3l1.m

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

TS=.1;
SIGNOISE=1.;
A0=1.;
A1=0.;
XH=0.;
XN=0.;
count=0;
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;
   	ArrayT(count)=T;
   	ArrayACT(count)=ACT;
   	ArrayXS(count)=XS;
   	ArrayXH(count)=XH;
   	ArrayXHERR(count)=XHERR;
   	ArraySP11(count)=SP11;
   	ArraySP11P(count)=SP11P;
   	ArrayEPS(count)=EPS;
end
figure
plot(ArrayT,ArrayACT,ArrayT,ArrayXS,ArrayT,ArrayXH),grid
xlabel('Time (Sec)')
ylabel('Measurement, Estimate & Signal')
axis([0 10 -2 4])
figure
plot(ArrayT,ArrayXHERR,ArrayT,ArraySP11,ArrayT,ArraySP11P),grid
xlabel('T (S)')
ylabel('Error in Estimate')
axis([0 10 -1 1])
clc
output=[ArrayT',ArrayACT',ArrayXS',ArrayXH'];
save datfil.txt output  -ascii
output=[ArrayT',ArrayXHERR',ArraySP11',ArraySP11P'];
save covfil.txt output  -ascii
disp 'simulation finished'