c6l2.m

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

ORDER=2;
T=0.;
S=0.;
H=.001;
TS=.1;
TF=10.;
PHIS=0.;
XJ=1.;
F=[0 1;0 0];
P=[100 0;0 100];
Q=[0 0;0 PHIS];
HMAT=[1 0];
HT=HMAT';
SIGN2=1.^2;
PHIN=SIGN2*TS;
count=0;
while T<=TF
	S=S+H;
	POLD=P;
	FP=F*P;
	PFT=FP';
 	PHT=P*HT;
	HP=HMAT*P;
	PHTHP=PHT*HP;
	PHTHPR=(1./PHIN)*PHTHP;
	PFTFP=PFT+FP;
 	PFTFPQ=PFTFP+Q;
	PD=PFTFPQ-PHTHPR;
	K=(1./PHIN)*PHT;
	HPD=(H)*PD;
	P=P+HPD;
 	T=T+H;
	FP=F*P;
	PFT=FP';
 	PHT=P*HT;
	HP=HMAT*P;
	PHTHP=PHT*HP;
	PHTHPR=(1./PHIN)*PHTHP;
	PFTFP=PFT+FP;
 	PFTFPQ=PFTFP+Q;
	PD=PFTFPQ-PHTHPR;
	K=(1./PHIN)*PHT;
	HPD=(H)*PD;
	PHPD=P+HPD;
	PPHPD=POLD+PHPD;
	P=(.5)*PPHPD;
 	if S>=(TS-.00001)
		S=0.;
		XK1=2.*(2.*XJ-1.)/(XJ*(XJ+1));
		XK2=6./(XJ*(XJ+1)*TS);
		P11DISC=2.*(2.*XJ-1)*SIGN2/(XJ*(XJ+1.));
		if XJ==1 
			P22DISC=0.;
		else
			P22DISC=12*SIGN2/(XJ*(XJ*XJ-1)*TS*TS);
		end
		K1TS=K(1,1)*TS;
		K2TS=K(2,1)*TS;
		P1=P(1,1);
		P2=P(2,2);
		count=count+1;
		ArrayT(count)=T;
		ArrayK1TS(count)=K1TS;
		ArrayK2TS(count)=K2TS;
		ArrayXK1(count)=XK1;
		ArrayXK2(count)=XK2;
		ArrayP1(count)=P1;
		ArrayP11DISC(count)=P11DISC;
		ArrayP2(count)=P2;
		ArrayP22DISC(count)=P22DISC;
		XJ=XJ+1.;
	end
end
figure
plot(ArrayT,ArrayK1TS,ArrayT,ArrayXK1),grid
xlabel('Time (Sec)')
ylabel('Continuous and Discrete First Kalman Gain')
axis([0 10 0 1.2])
figure
plot(ArrayT,ArrayK2TS,ArrayT,ArrayXK2),grid
xlabel('Time (Sec)')
ylabel('Continuous and Discrete Second Kalman Gain')
axis([0 10 0 8])
figure
plot(ArrayT,ArrayP1,ArrayT,ArrayP11DISC),grid
xlabel('Time (Sec)')
ylabel('P11')
axis([0 10 0 1.2])
figure
plot(ArrayT,ArrayP2,ArrayT,ArrayP22DISC),grid
xlabel('Time (Sec)')
ylabel('P22')
axis([0 10 0 10])
clc
output=[ArrayT',ArrayK1TS',ArrayXK1',ArrayK2TS',ArrayXK2'];
save datfil.txt output  -ascii
output=[ArrayT',ArrayP1',ArrayP11DISC',ArrayP2',ArrayP22DISC'];
save covfil.txt output  -ascii
disp 'simulation finished'