c6l1.m

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

ORDER=1;
T=0.;
S=0.;
H=.001;
TS=.1;
TF=10.;
PHIS=0.;
XJ=1.;
F=[0];
P=[100];
Q=[PHIS];
HMAT=[1];
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=1./XJ;
		PDISC=SIGN2/XJ;
		KTS=K(1,1)*TS;
		count=count+1;
		ArrayT(count)=T;
		ArrayKTS(count)=KTS;
		ArrayXK1(count)=XK1;
		ArrayPDISC(count)=PDISC;
		ArrayP(count)=P;
		XJ=XJ+1.;
	end
end
figure
plot(ArrayT,ArrayKTS,ArrayT,ArrayXK1),grid
xlabel('Time (Sec)')
ylabel('Continuous and Discrete Kalman Gain')
axis([0 10 0 .5])
figure
plot(ArrayT,ArrayPDISC,ArrayT,ArrayP),grid
xlabel('Time (Sec)')
ylabel('Continuous and Discrete Covariance')
axis([0 10 0 .5])
clc
output=[ArrayT',ArrayKTS',ArrayXK1'];
save datfil.txt output  -ascii
disp 'simulation finished'