c4l2.m

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

ORDER=3;
TS=1.;
SIGNOISE=1.;
PHI=[1 TS .5*TS*TS;0 1 TS;0 0 1];
P=[99999999 0 0;0 999999999 0;0 0 999999999];
IDNP=eye(ORDER);
H=[1 0 0];
HT=H';
R=SIGNOISE^2;
PHIT=PHI';
count=0;
for XN=1:100
	PHIP=PHI*P;
	M=PHIP*PHIT;
	MHT=M*HT;
	HMHT=H*MHT;
	HMHTR=HMHT+R;
	HMHTRINV=inv(HMHTR);
	K=MHT*HMHTRINV;
	KH=K*H;
	IKH=IDNP-KH;
	P=IKH*M;
	if XN<3
		P11GM=9999999999.;
		P22GM=9999999999.;
		P33GM=9999999999.;
	else
		P11GM=(3*(3*XN*XN-3*XN+2)/(XN*(XN+1)*(XN+2)))*SIGNOISE^2;
		P22GM=(12*(16*XN*XN-30*XN+11)/(XN*(XN*XN-1)*(XN*XN-4)*TS*TS))*SIGNOISE^2;
		P33GM=(720/(XN*(XN*XN-1)*(XN*XN-4)*TS*TS*TS*TS))*SIGNOISE^2;
	end
	SP11=sqrt(P(1,1));
	SP22=sqrt(P(2,2));
	SP33=sqrt(P(3,3));
	SP11GM=sqrt(P11GM);
	SP22GM=sqrt(P22GM);
	SP33GM=sqrt(P33GM);
	K1GM=3*(3*XN*XN-3*XN+2)/(XN*(XN+1)*(XN+2));
	K2GM=18*(2*XN-1)/(XN*(XN+1)*(XN+2)*TS);
	K3GM=60/(XN*(XN+1)*(XN+2)*TS*TS);
	K1=K(1,1);
	K2=K(2,1);
	K3=K(3,1);
	if XN>=3
		count=count+1;
   		ArrayXN(count)=XN;
   		ArrayK1(count)=K1;
   		ArrayK1GM(count)=K1GM;
   		ArrayK2(count)=K2;
   		ArrayK2GM(count)=K2GM;
   		ArrayK3(count)=K3;
   		ArrayK3GM(count)=K3GM;
   		ArraySP11(count)=SP11;
   		ArraySP11GM(count)=SP11GM;
   		ArraySP22(count)=SP22;
   		ArraySP22GM(count)=SP22GM;
   		ArraySP33(count)=SP33;
   		ArraySP33GM(count)=SP33GM;
	end
end
figure
plot(ArrayXN,ArraySP11,ArrayXN,ArraySP11GM),grid
xlabel('Number of Measurements')
ylabel('Error in Estimate of First State')
axis([0 100 0 1])
figure
plot(ArrayXN,ArraySP22,ArrayXN,ArraySP22GM),grid
title('Error in Estimate of Second State')
xlabel('Number of Measurements')
ylabel('Error in Estimate of Second State')
axis([0 100 0 .5])
figure
plot(ArrayXN,ArraySP33,ArrayXN,ArraySP33GM),grid
title('Error in Estimate of Third State')
xlabel('Number of Measurements')
ylabel('Error in Estimate of Third State')
axis([0 100 0 .1])
figure
plot(ArrayXN,ArrayK1,ArrayXN,ArrayK1GM),grid
xlabel('Number of Measurements')
ylabel('First Kalman Gain')
axis([0 100 0 1])
figure
plot(ArrayXN,ArrayK2,ArrayXN,ArrayK2GM),grid
xlabel('Number of Measurements')
ylabel('Second Kalman Gain')
axis([0 100 0 .5])
figure
plot(ArrayXN,ArrayK3,ArrayXN,ArrayK3GM),grid
xlabel('Number of Measurements')
ylabel('Third Kalman Gain')
axis([0 100 0 .1])
clc
output=[ArrayXN',ArrayK1',ArrayK1GM',ArrayK2',ArrayK2GM',ArrayK3',ArrayK3GM'];
save datfil.txt output  -ascii
output=[ArrayXN',ArraySP11',ArraySP11GM',ArraySP22',ArraySP22GM',ArraySP33',ArraySP33GM'];
save covfil.txt output  -ascii
disp 'simulation finished'