c13l2.m

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

X=200000.;
XD=-6000.;
BETA=500.;
XNOM=200000.;
XDNOM=-6000.;
BETANOM=500.;
ORDER=3;
TS=.1;
TF=30.;
Q33=300.*300./TF;
T=0.;
S=0.;
H=.001;
SIGNOISE=25.;
RMAT(1,1)=SIGNOISE^2;
XH(1,1)=X-XNOM;
XH(2,1)=XD-XDNOM;
XH(3,1)=-300.;
PHI=zeros(ORDER,ORDER);
P=zeros(ORDER,ORDER);
IDNP=eye(ORDER);
Q=zeros(ORDER,ORDER);
P(1,1)=SIGNOISE*SIGNOISE;
P(2,2)=20000.;
P(3,3)=300.^2;
HMAT=zeros(1,ORDER);
HMAT(1,1)=1.;
HT=HMAT';
count=0;
while X>=0.
	XOLD=X;
	XDOLD=XD;
	XNOMOLD=XNOM;
	XDNOMOLD=XDNOM;
	XDD=.0034*32.2*XD*XD*exp(-X/22000.)/(2.*BETA)-32.2;
	XDDNOM=.0034*32.2*XDNOM*XDNOM*exp(-XNOM/22000.)/(2.*BETANOM)-32.2;
 	X=X+H*XD;
	XD=XD+H*XDD;
	XNOM=XNOM+H*XDNOM;
	XDNOM=XDNOM+H*XDDNOM;
 	T=T+H;
	XDD=.0034*32.2*XD*XD*exp(-X/22000.)/(2.*BETA)-32.2;
	XDDNOM=.0034*32.2*XDNOM*XDNOM*exp(-XNOM/22000.)/(2.*BETANOM)-32.2;
 	X=.5*(XOLD+X+H*XD);
	XD=.5*(XDOLD+XD+H*XDD);
	XNOM=.5*(XNOMOLD+XNOM+H*XDNOM);
	XDNOM=.5*(XDNOMOLD+XDNOM+H*XDDNOM);
 	S=S+H;
	if S>=(TS-.00001)
		S=0.;
		RHONOM=.0034*exp(-XNOM/22000.);
		F21=-32.2*RHONOM*XDNOM*XDNOM/(2.*22000.*BETANOM);
		F22=RHONOM*32.2*XDNOM/BETANOM;
		F23=-RHONOM*32.2*XDNOM*XDNOM/(2.*BETANOM*BETANOM);
		PHI(1,1)=1.;
		PHI(1,2)=TS;
		PHI(2,1)=F21*TS;
		PHI(2,2)=1.+F22*TS;
		PHI(2,3)=F23*TS;
		PHI(3,3)=1.;
		Q(2,2)=F23*F23*Q33*TS*TS*TS/3.;
		Q(2,3)=F23*Q33*TS*TS/2.;
		Q(3,2)=Q(2,3);
		Q(3,3)=Q33*TS;
		PHIT=PHI';
 		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;
		DELX=X-XNOM;
		MEAS(1,1)=DELX+XNOISE;
		PHIXH=PHI*XH;
		HPHIXH=HMAT*PHIXH;
		RES=MEAS-HPHIXH;
		KRES=K*RES;
		XH=PHIXH+KRES;
		DELXH=XH(1,1);
		DELXDH=XH(2,1);
		DELBETAH=XH(3,1);
		XHH=XNOM+DELXH;
		XDH=XDNOM+DELXDH;
		BETAH=BETANOM+DELBETAH;
		ERRX=X-XHH;
		SP11=sqrt(P(1,1));
		ERRXD=XD-XDH;
		SP22=sqrt(P(2,2));
		ERRBETA=BETA-BETAH;
		SP33=sqrt(P(3,3));
		SP11P=-SP11;
		SP22P=-SP22;
		SP33P=-SP33;
		count=count+1;
		ArrayT(count)=T;
		ArrayBETA(count)=BETA;
		ArrayBETAH(count)=BETAH;
		ArrayERRBETA(count)=ERRBETA;
		ArraySP33(count)=SP33;
		ArraySP33P(count)=SP33P;
	end
end
figure
plot(ArrayT,ArrayBETA,ArrayT,ArrayBETAH),grid
xlabel('Time (Sec)')
ylabel('Ballistic Coefficient (Lb/Ft^2)')
axis([0 50 0 800])
figure
plot(ArrayT,ArrayERRBETA,ArrayT,ArraySP33,ArrayT,ArraySP33P),grid
xlabel('Time (Sec)')
ylabel('Error in Estimate of Ballistic Coefficient (Lb/Ft^2)')
axis([0 50 -350 350])
clc
output=[ArrayT',ArrayBETA',ArrayBETAH',ArrayERRBETA',ArraySP33',ArraySP33P'];
save datfil.txt output  -ascii
disp 'simulation finished'