c4l1.m

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

ORDER=2;
TS=1.;
SIGNOISE=1.;
PHI=[1 TS;0 1];
P=[99999999 0;0 999999999];
IDNP=eye(ORDER);
H=[1 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<2
			P11GM=9999999999.;
			P22GM=9999999999.;
		else
			P11GM=2.*(2.*XN-1)*SIGNOISE*SIGNOISE/(XN*(XN+1.));
			P22GM=12.*SIGNOISE*SIGNOISE/(XN*(XN*XN-1.)*TS*TS);
		end
		SP11=sqrt(P(1,1));
		SP22=sqrt(P(2,2));
		SP11GM=sqrt(P11GM);
		SP22GM=sqrt(P22GM);
		K1GM=2.*(2.*XN-1.)/(XN*(XN+1.));
		K2GM=6./(XN*(XN+1.)*TS);
		K1=K(1,1);
		K2=K(2,1);
		count=count+1;
   		ArrayXN(count)=XN;
   		ArrayK1(count)=K1;
   		ArrayK1GM(count)=K1GM;
   		ArrayK2(count)=K2;
   		ArrayK2GM(count)=K2GM;
   		ArraySP11(count)=SP11;
   		ArraySP11GM(count)=SP11GM;
   		ArraySP22(count)=SP22;
   		ArraySP22GM(count)=SP22GM;
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
xlabel('Number of Measurements')
ylabel('Error in Estimate of Second 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 .1])
clc
output=[ArrayXN',ArrayK1',ArrayK1GM',ArrayK2',ArrayK2GM'];
save datfil.txt output  -ascii
output=[ArrayXN',ArraySP11',ArraySP11GM',ArraySP22',ArraySP22GM'];
save covfil.txt output  -ascii
disp 'simulation finished'