c3l4.m

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

TS=.1;
SIGNOISE=50;
A0=2;
A1=-2;
A2=5;
A3=0;
XH=0;
XDH=0;
XDDH=0;
XN=0.;
count=0;
for T=0:TS:10;
	XN=XN+1.;
	XNOISE=SIGNOISE*randn;
	X=A0+A1*T+A2*T*T+A3*T*T*T;
	XD=A1+2*A2*T+3.*A3*T*T;
	XDD=2*A2+6*A3*T;
	XS=X+XNOISE;
	XK1=3*(3*XN*XN-3*XN+2)/(XN*(XN+1)*(XN+2));
	XK2=18*(2*XN-1)/(XN*(XN+1)*(XN+2)*TS);
	XK3=60/(XN*(XN+1)*(XN+2)*TS*TS);
	RES=XS-XH-TS*XDH-.5*TS*TS*XDDH;
	XH=XH+XDH*TS+.5*XDDH*TS*TS+XK1*RES;
	XDH=XDH+XDDH*TS+XK2*RES;
	XDDH=XDDH+XK3*RES;
	if XN==1
		SP11=0;
		SP22=0;
		SP33=0;
	elseif XN==2
		SP11=0;
		SP22=0;
		SP33=0;
	else
		SP11=SIGNOISE*sqrt(3*(3*XN*XN-3*XN+2)/(XN*(XN+1)*(XN+2)));
		SP22=SIGNOISE*sqrt(12*(16*XN*XN-30*XN+11)/(XN*(XN*XN-1)*(XN*XN-4)*TS*TS));
		SP33=SIGNOISE*sqrt(720/(XN*(XN*XN-1)*(XN*XN-4)*TS*TS*TS*TS));
	end
   	XHERR=X-XH;
	XDHERR=XD-XDH;
	XDDHERR=XDD-XDDH;
	EPS=A3*TS*TS*TS*(XN-1)*(XN-2)*(XN-3)/20;
	EPSD=A3*TS*TS*(6*XN*XN-15*XN+11)/10;
	EPSDD=3*A3*TS*(XN-1);
	SP11P=-SP11;
	SP22P=-SP22;
	SP33P=-SP33;
   	count=count+1;
   	ArrayT(count)=T;
   	ArrayX(count)=X;
   	ArrayXS(count)=XS;
   	ArrayXH(count)=XH;
   	ArrayXD(count)=XD;
   	ArrayXDH(count)=XDH;
   	ArrayXDD(count)=XDD;
   	ArrayXDDH(count)=XDDH;
   	ArrayXHERR(count)=XHERR;
   	ArraySP11(count)=SP11;
   	ArraySP11P(count)=SP11P;
   	ArrayXDHERR(count)=XDHERR;
   	ArraySP22(count)=SP22;
   	ArraySP22P(count)=SP22P;
   	ArrayXDDHERR(count)=XDDHERR;
   	ArraySP33(count)=SP33;
   	ArraySP33P(count)=SP33P;
end;
figure
plot(ArrayT,ArrayX,ArrayT,ArrayXS,ArrayT,ArrayXH),grid
xlabel('Time (Sec)')
ylabel('Measurement, Estimate & Signal')
axis([0 10 -100 500])
figure
plot(ArrayT,ArrayXD,ArrayT,ArrayXDH),grid
xlabel('Time (Sec)')
ylabel('XD & XDH')
axis([0 10 -200 200])
figure
plot(ArrayT,ArrayXDD,ArrayT,ArrayXDDH),grid
xlabel('Time (Sec)')
ylabel('XDD & XDDH')
axis([0 10 -50 50])
figure
plot(ArrayT,ArrayXHERR,ArrayT,ArraySP11,ArrayT,ArraySP11P),grid
xlabel('Time (Sec)')
ylabel('Error in Estimate of x')
axis([0 10 -60 60])
figure
plot(ArrayT,ArrayXDHERR,ArrayT,ArraySP22,ArrayT,ArraySP22P),grid
xlabel('Time (Sec)')
ylabel('Error in Estimate of x Dot')
axis([0 10 -200 200])
figure
plot(ArrayT,ArrayXDDHERR,ArrayT,ArraySP33,ArrayT,ArraySP33P),grid
xlabel('Time (Sec)')
ylabel('Error in Estimate of x Double Dot')
axis([0 10 -50 50])
clc
output=[ArrayT',ArrayX',ArrayXH',ArrayXD',ArrayXDH',ArrayXDD',ArrayXDDH'];
save datfil.txt output  -ascii
output=[ArrayT',ArrayXHERR',ArraySP11',ArraySP11P'];
save covfil.txt output  -ascii
disp 'simulation finished'