c6l3.m

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

ORDER=3;
T=0.;
S=0.;
H=.001;
TS=.1;
TF=10.;
PHIS=0.;
XJ=1.;
F=[0 1 0;0 0 1;0 0 0];
Q=[0 0 0;0 0 0;0 0 PHIS];
P=[100 0 0;0 100 0;0 0 100];
HMAT=[1 0 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=3.*(3*XJ*XJ-3.*XJ+2.)/(XJ*(XJ+1)*(XJ+2));
		XK2=18.*(2.*XJ-1.)/(XJ*(XJ+1)*(XJ+2)*TS);
		XK3=60./(XJ*(XJ+1)*(XJ+2)*TS*TS);
		P11DISC=3*(3*XJ*XJ-3*XJ+2)*SIGN2/(XJ*(XJ+1)*(XJ+2));
		if XJ==1
			P22DISC=0.;
			P33DISC=0.;
		elseif XJ==2
			P22DISC=0.;
			P33DISC=0.;
		else
			P22DISC=12*(16*XJ*XJ-30*XJ+11)*SIGN2/(XJ*(XJ*XJ-1)*(XJ*XJ-2)*TS*TS);
			P33DISC=720*SIGN2/(XJ*(XJ*XJ-1)*(XJ*XJ-2)*TS^4);
		end
		K1TS=K(1,1)*TS;
		K2TS=K(2,1)*TS;
		K3TS=K(3,1)*TS;
		P1=P(1,1);
		P2=P(2,2);
		P3=P(3,3);
		count=count+1;
		ArrayT(count)=T;
		ArrayK1TS(count)=K1TS;
		ArrayK2TS(count)=K2TS;
		ArrayK3TS(count)=K3TS;
		ArrayXK1(count)=XK1;
		ArrayXK2(count)=XK2;
		ArrayXK3(count)=XK3;
		ArrayP1(count)=P1;
		ArrayP11DISC(count)=P11DISC;
		ArrayP2(count)=P2;
		ArrayP22DISC(count)=P22DISC;
		ArrayP3(count)=P3;
		ArrayP33DISC(count)=P33DISC;
		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 10])
figure
plot(ArrayT,ArrayK3TS,ArrayT,ArrayXK3),grid
xlabel('Time (Sec)')
ylabel('Continuous and Discrete Third Kalman Gain')
axis([0 10 0 5])
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 5])
figure
plot(ArrayT,ArrayP3,ArrayT,ArrayP33DISC),grid
xlabel('Time (Sec)')
ylabel('P33')
axis([0 10 0 5])
clc
output=[ArrayT',ArrayK1TS',ArrayXK1',ArrayK2TS',ArrayXK2',ArrayK3TS',ArrayXK3'];
save datfil.txt output  -ascii
disp 'simulation finished'