c3l3.m

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

TS=.1;
SIGNOISE=5.;
A0=3.;
A1=1.;
A2=0.;
XH=0.;
XDH=0.;
XN=0;
count=0;
for T=0:TS:10
	XN=XN+1.;
	XNOISE=SIGNOISE*randn;
	X=A0+A1*T+A2*T*T;
	XD=A1+2*A2*T;
	XS=X+XNOISE;
	XK1=2*(2*XN-1)/(XN*(XN+1));
	XK2=6/(XN*(XN+1)*TS);
	RES=XS-XH-TS*XDH;
	XH=XH+XDH*TS+XK1*RES;
	XDH=XDH+XK2*RES;
	if XN==1
		SP11=0;
		SP22=0;
	else
		SP11=SIGNOISE*sqrt(2.*(2*XN-1)/(XN*(XN+1)));
		SP22=SIGNOISE*sqrt(12/(XN*(XN*XN-1)*TS*TS));
	end
	XHERR=X-XH;
	XDHERR=XD-XDH;
	EPS=A2*TS*TS*(XN-1)*(XN-2)/6;
	EPSD=A2*TS*(XN-1);
	SP11P=-SP11;
	SP22P=-SP22;
	count=count+1;
   	ArrayT(count)=T;
   	ArrayX(count)=X;
   	ArrayXS(count)=XS;
   	ArrayXH(count)=XH;
   	ArrayXD(count)=XD;
   	ArrayXDH(count)=XDH;
   	ArrayXHERR(count)=XHERR;
   	ArraySP11(count)=SP11;
   	ArraySP11P(count)=SP11P;
   	ArrayXDHERR(count)=XDHERR;
   	ArraySP22(count)=SP22;
   	ArraySP22P(count)=SP22P;
end
figure
plot(ArrayT,ArrayX,ArrayT,ArrayXS,ArrayT,ArrayXH),grid
xlabel('T (S)')
ylabel('Measurement, Signal & Estimate')
axis([0 10 -5 25])
figure
plot(ArrayT,ArrayXD,ArrayT,ArrayXDH),grid
xlabel('Time (Sec)')
ylabel('XD & XDH')
axis([0 10 -5 35])
figure
plot(ArrayT,ArrayXHERR,ArrayT,ArraySP11,ArrayT,ArraySP11P),grid
xlabel('Time (Sec)')
ylabel('Error in Estimate of x')
axis([0 10 -5 5])
figure
plot(ArrayT,ArrayXDHERR,ArrayT,ArraySP22,ArrayT,ArraySP22P),grid
xlabel('Time (Sec)')
ylabel('Error in Estimate of x Dot')
axis([0 10 -10 10])
clc
output=[ArrayT',ArrayX',ArrayXH',ArrayXD',ArrayXDH'];
save datfil.txt output  -ascii
output=[ArrayT',ArrayXHERR',ArraySP11',ArraySP11P',ArrayXDHERR',ArraySP22',ArraySP22P'];
save covfil.txt output  -ascii
disp 'simulation finished'