c14l4.m

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

TS=.1;
PHIS=0.;
A0=0.;
A1=1000.;
A2=193.2/2.;
XH=0.;
XDH=0.;
SIGNOISE=10.;
ORDER=2;
T=0.;
S=0.;
H=.001;
PHI=zeros(ORDER,ORDER);
P=zeros(ORDER,ORDER);
IDNP=eye(ORDER);
Q=zeros(ORDER,ORDER);
RMAT(1,1)=SIGNOISE^2;
P(1,1)=99999999999.;
P(2,2)=99999999999.;
PHI(1,1)=1.;
PHI(1,2)=TS;
PHI(2,2)=1.;
Q(1,1)=TS*TS*TS*PHIS/3.;
Q(1,2)=.5*TS*TS*PHIS;
Q(2,1)=Q(1,2);
Q(2,2)=PHIS*TS;
HMAT(1,1)=1.;
HMAT(1,2)=0.;
count=0;
for T=0:TS:10
	PHIT=PHI';
	HT=HMAT';
	PHIP=PHI*P;
	PHIPPHIT=PHIP*PHIT;
	M=PHIPPHIT+Q;
	HM=HMAT*M;
	HMHT=HM*HT;
	HMHTR=HMHT+RMAT;
	HMHTRINV(1,1)=1./HMHTR(1,1);
	MHT=M*HT;
	K=MHT*HMHTRINV;
	KH=K*HMAT;
	IKH=IDNP-KH;
	P=IKH*M;
	XNOISE=SIGNOISE*randn;
	X=A0+A1*T+A2*T*T;
	XD=A1+2*A2*T;
	XS=X+XNOISE;
	RES=XS-XH-TS*XDH;
	XH=XH+XDH*TS+K(1,1)*RES;
	XDH=XDH+K(2,1)*RES;
	SP11=sqrt(P(1,1));
	SP22=sqrt(P(2,2));
	SP44=sqrt(HMHTR(1,1));
	XHERR=X-XH;
	XDHERR=XD-XDH;
	SP11P=-SP11;
	SP22P=-SP22;
	SP44P=-SP44;
	count=count+1;
	ArrayT(count)=T;
	ArrayX(count)=X;
	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;
	ArrayRES(count)=RES;
	ArraySP44(count)=SP44;
	ArraySP44P(count)=SP44P;
	end
figure
plot(ArrayT,ArrayXHERR,ArrayT,ArraySP11,ArrayT,ArraySP11P),grid
xlabel('Time (Sec)')
ylabel('Error in Estimate of Position (Ft)')
axis([0 10 -100 100])
figure
plot(ArrayT,ArrayRES,ArrayT,ArraySP44,ArrayT,ArraySP44P),grid
xlabel('Time (Sec)')
ylabel('Residual')
axis([0 10 -100 100])
clc
output=[ArrayT',ArrayX',ArrayXH',ArrayXD',ArrayXDH'];
save datfil.txt output  -ascii
output=[ArrayT',ArrayXHERR',ArraySP11',ArraySP11P',ArrayXDHERR',ArraySP22',ArraySP22P'...
,ArrayRES',ArraySP44',ArraySP44P'];
save covfil.txt output  -ascii
disp 'simulation finished'