c14l9.m

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

ORDER =3;
ORDERPZ=1;
XLOSE=100.;
W=.1;
PHIS=1.;
PHISPZ=10.;
TS=1.;
XH=0;
XDH=0;
XDDH=0;
BIASH=0.;
BIAS=1.;
SIGNOISE=50.;
PHI=zeros(ORDER,ORDER);
P=zeros(ORDER,ORDER);
IDNP=eye(ORDER);
Q=zeros(ORDER,ORDER);
IDNPZ(1,1)=1.;
P(1,1)=99999999999999.;
P(2,2)=99999999999999.;
P(3,3)=99999999999999.;
PPZ(1,1)=99999999999999.;
PHI(1,1)=1;
PHI(1,2)=TS;
PHI(1,3)=.5*TS*TS;
PHI(2,2)=1;
PHI(2,3)=TS;
PHI(3,3)=1;
PHIPZ(1,1)=1;
H=zeros(1,ORDER);
H(1,1)=1;
HT=H';
R(1,1)=SIGNOISE^2;
PHIT=PHI';
PHITPZ=PHIPZ';
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;
QPZ(1,1)=PHISPZ*TS;
count=0;
for T=0:TS:200
	if T>XLOSE 
		R(1,1)=99999999999999.;
	end
	PHIP=PHI*P;
	PHIPPHIT=PHIP*PHIT;
	M=PHIPPHIT+Q;
	MHT=M*HT;
	HMHT=H*MHT;
	HMHTR(1,1)=HMHT(1,1)+R(1,1);
	HMHTRINV(1,1)=1./HMHTR(1,1);
	K=MHT*HMHTRINV;
	KH=K*H;
	IKH=IDNP-KH;
	P=IKH*M;
	HPZ(1,1)=.5*T*T;
	HTPZ=HPZ';
	PHIPPZ=PHIPZ*PPZ;
	PHIPPHITPZ=PHIPPZ*PHITPZ;
	MPZ=PHIPPHITPZ+QPZ;
	MHTPZ=MPZ*HTPZ;
	HMHTPZ=HPZ*MHTPZ;
	HMHTRPZ(1,1)=HMHTPZ(1,1)+R(1,1);
	HMHTRINVPZ(1,1)=1./HMHTRPZ(1,1);
	KPZ=MHTPZ*HMHTRINVPZ;
	KHPZ=KPZ*HPZ;
	IKHPZ=IDNPZ-KHPZ;
	PPZ=IKHPZ*MPZ;
	XNOISE=SIGNOISE*randn;
	X=100.*T-20.*cos(W*T)/W+20./W;
	XD=100.+20.*sin(W*T);
	XDD=20.*W*cos(W*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;
	XGPSS=XS;
	XINSS=X+.5*BIAS*T*T;
	XSPZ=XINSS-XGPSS;
	RESPZ=XSPZ-.5*BIASH*T*T;
	BIASH=BIASH+KPZ(1,1)*RESPZ;
	if T>XLOSE
		XH=XINSS-.5*BIASH*T*T;
	end
	SP11=sqrt(P(1,1));
	SP22=sqrt(P(2,2));
	SP33=sqrt(P(3,3));
	SP11P=-SP11;
	SP22P=-SP22;
	SP33P=-SP33;
	XHERR=X-XH;
	XDHERR=XD-XDH;
	XDDHERR=XDD-XDDH;
	SP11PZ=sqrt(PPZ(1,1));
	SP11PZP=-SP11PZ;
	BIASERR=BIAS-BIASH;
	count=count+1;
	ArrayT(count)=T;
	ArrayX(count)=X;
	ArrayXH(count)=XH;
	ArrayXD(count)=XD;
	ArrayXDH(count)=XDH;
	ArrayXDD(count)=XDD;
	ArrayXDDH(count)=XDDH;
	ArrayBIAS(count)=BIAS;
	ArrayBIASH(count)=BIASH;
	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;
	ArrayBIASERR(count)=BIASERR;
	ArraySP11PZ(count)=SP11PZ;
	ArraySP11PZP(count)=SP11PZP;
end
figure
plot(ArrayT,ArrayXHERR),grid
xlabel('Time (Sec)')
ylabel('Error in Estimate of Position (Ft)')
axis([0 200 -100 200])
clc
output=[ArrayT',ArrayX',ArrayXH',ArrayXD',ArrayXDH',ArrayXDD',ArrayXDDH',...
ArrayBIAS',ArrayBIASH'];
save datfil.txt output  -ascii
output=[ArrayT',ArrayXHERR',ArraySP11',ArraySP11P',ArrayXDHERR',ArraySP22',...
ArraySP22P',ArrayXDDHERR',ArraySP33',ArraySP33P',ArrayBIASERR',ArraySP11PZ',...
ArraySP11PZP'];
save covfil.txt output  -ascii
disp 'simulation finished'