c5l2.m

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

ORDER =3;
PHIS=0.;
TS=.1;
XH=0.;	
XDH=0.;
XDDH=0;
SIGNOISE=1.;
PHI=[1 TS .5*TS*TS;0 1 TS;0 0 1];
P=[99999999 0 0;0 999999999 0;0 0 999999999];
IDNP=eye(ORDER);
Q=zeros(ORDER); 
RMAT=SIGNOISE^2;
HMAT=[1 0 0];
HT=HMAT';
PHIT=PHI';
Q(1,1)=PHIS*TS^5/20;
Q(1,2)=PHIS*TS^4/8;
Q(1,3)=PHIS*TS^3/6;
Q(2,1)=Q(1,2);
Q(2,2)=PHIS*TS^3/3;
Q(2,3)=PHIS*TS*TS/2;
Q(3,1)=Q(1,3);
Q(3,2)=Q(2,3);
Q(3,3)=PHIS*TS;
count=0;
for T=0:TS:20
	PHIP=PHI*P;
	PHIPPHIT=PHIP*PHIT;
	M=PHIPPHIT+Q;
	HM=HMAT*M;
	HMHT=HM*HT;
	HMHTR=HMHT+RMAT;
	HMHTRINV=inv(HMHTR);
	MHT=M*HT;
	K=MHT*HMHTRINV;
	KH=K*HMAT;
	IKH=IDNP-KH;
	P=IKH*M;
	XNOISE=SIGNOISE*randn;
	X=sin(T);
	XD=cos(T);
	XDD=-sin(T);
	XS=X+XNOISE;
	RES=XS-XH-TS*XDH-.5*TS*TS*XDDH;
	XH=XH+XDH*TS+.5*TS*TS*XDDH+K(1,1)*RES;
	XDH=XDH+XDDH*TS+K(2,1)*RES;
	XDDH=XDDH+K(3,1)*RES;
	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;
end
figure
plot(ArrayT,ArrayX,ArrayT,ArrayXH),grid
xlabel('Time (Sec)')
ylabel('Estimate and True Signal')
axis([0 20 -2 2])
figure
plot(ArrayT,ArrayXD,ArrayT,ArrayXDH),grid
xlabel('Time (Sec)')
ylabel('XD and XDH')
axis([0 20 -4 4])	
clc
output=[ArrayT',ArrayX',ArrayXH',ArrayXD',ArrayXDH',ArrayXDD',ArrayXDDH'];
save datfil.txt output  -ascii
disp 'simulation finished'