c14l2.m

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

STATE=3;
NPTS=5;
PHIS=0.;
TS=1.;
SIGNOISE=1.;
PHI=zeros(STATE,STATE);
P=zeros(STATE,STATE);
IDNP=eye(STATE);
Q=zeros(STATE,STATE);
H=zeros(1,STATE);
H(1,1)=1;
HT=H';
R(1,1)=SIGNOISE^2;
if STATE==1
	XH(1,1)=0;
	P(1,1)=99999999999999.;
	PHI(1,1)=1;
elseif STATE==2
	XH(1,1)=0;
	XH(2,1)=0;
	P(1,1)=99999999999999.;
	P(2,2)=99999999999999.;
	PHI(1,1)=1;
	PHI(1,2)=TS;
	PHI(2,2)=1;
elseif STATE==3
	XH(1,1)=0;
	XH(2,1)=0;
	XH(3,1)=0;
	P(1,1)=99999999999999.;
	P(2,2)=99999999999999.;
	P(3,3)=99999999999999.;
	PHI(1,1)=1;
	PHI(1,2)=TS;
	PHI(1,3)=.5*TS*TS;
	PHI(2,2)=1;
	PHI(2,3)=TS;
	PHI(3,3)=1;
elseif STATE==4
	XH(1,1)=0;
	XH(2,1)=0;
	XH(3,1)=0;
	XH(4,1)=0;
	P(1,1)=99999999999999.;
	P(2,2)=99999999999999.;
	P(3,3)=99999999999999.;
	P(4,4)=99999999999999.;
	PHI(1,1)=1;
	PHI(1,2)=TS;
	PHI(1,3)=.5*TS*TS;
	PHI(1,4)=TS*TS*TS/6.;
	PHI(2,2)=1;
	PHI(2,3)=TS;
	PHI(2,4)=.5*TS*TS;
	PHI(3,3)=1.;
	PHI(3,4)=TS;
	PHI(4,4)=1.;
else
	XH(1,1)=0;
	XH(2,1)=0;
	XH(3,1)=0;
	XH(4,1)=0;
	XH(5,1)=0;
	P(1,1)=99999999999999.;
	P(2,2)=99999999999999.;
	P(3,3)=99999999999999.;
	P(4,4)=99999999999999.;
	P(5,5)=99999999999999.;
	PHI(1,1)=1;
	PHI(1,2)=TS;
	PHI(1,3)=.5*TS*TS;
	PHI(1,4)=TS*TS*TS/6.;
	PHI(1,5)=TS*TS*TS*TS/24.;
	PHI(2,2)=1;
	PHI(2,3)=TS;
	PHI(2,4)=.5*TS*TS;
	PHI(2,5)=TS*TS*TS/6.;
	PHI(3,3)=1.;
	PHI(3,4)=TS;
	PHI(3,5)=.5*TS*TS;
	PHI(4,4)=1.;
	PHI(4,5)=TS;
	PHI(5,5)=1.;
end
PHIT=PHI';
count=0;
for I=1:NPTS
	T=(I-1)*TS;
	PHIP=PHI*P;
	PHIPPHIT=PHIP*PHIT;
	M=PHIPPHIT+Q;
	MHT=M*HT;
	HMHT=H*MHT;
	HMHTR(1,1)=HMHT(1,1)+R(1,1);
	HMHTRINV(1,1)=1./HMHTR(1,1);
	K=MHT*HMHTRINV;
	KH=K*H;
	IKH=IDNP-KH;
	P=IKH*M;
	XNOISE=SIGNOISE*randn;
	XNOISE=0.;
	X=T^2+15*T-3.;
	XD=2.*T+15.;
	XDD=2.;
	XDDD=0;
	XDDDD=0.;
	XS=X+XNOISE;
	PHIX=PHI*XH;
	HPHI=H*PHI;
	HPHIX=HPHI*XH;
	RES=XS-HPHIX(1,1);
	KRES=RES*K;
	XH=PHIX+KRES;
	count=count+1;
	XHAT=XH(1,1);
	ArrayT(count)=T;
	ArrayX(count)=X;
	ArrayXHAT(count)=XHAT;
	if STATE>1
		XDHAT=XH(2,1);
		ArrayXD(count)=XD;
		ArrayXDHAT(count)=XDHAT;
	end
	if STATE>2
		XDDHAT=XH(3,1);
		ArrayXDD(count)=XDD;
		ArrayXDDHAT(count)=XDDHAT;
	end
	if STATE>3
		XDDDHAT=XH(4,1);
		ArrayXDDD(count)=XDDD;
		ArrayXDDDHAT(count)=XDDDHAT;
	end
	if STATE>3
		XDDDDHAT=XH(5,1);
		ArrayXDDDD(count)=XDDDD;
		ArrayXDDDDHAT(count)=XDDDDHAT;
	end
end
figure
plot(ArrayT,ArrayX,ArrayT,ArrayXHAT),grid
xlabel('Number of Measurements')
ylabel('Measurement and Estimate')
if STATE>1
	figure
	plot(ArrayT,ArrayXD,ArrayT,ArrayXDHAT),grid
	xlabel('Number of Measurements')
	ylabel('XD and XDHAT')
end
if STATE>2
	figure
	plot(ArrayT,ArrayXDD,ArrayT,ArrayXDDHAT),grid
	xlabel('Number of Measurements')
	ylabel('XDD and XDDHAT')
end
if STATE>3
	figure
	plot(ArrayT,ArrayXDDD,ArrayT,ArrayXDDDHAT),grid
	xlabel('Number of Measurements')
	ylabel('XDDD and XDDDHAT')
end
if STATE>4
	figure
	plot(ArrayT,ArrayXDDDD,ArrayT,ArrayXDDDDHAT),grid
	xlabel('Number of Measurements')
	ylabel('XDDDD and XDDDDHAT')
end
clc
if STATE==1
	output=[ArrayT',ArrayX',ArrayXHAT'];
	save datfil.txt output  -ascii
	disp 'simulation finished'
elseif STATE==2
	output=[ArrayT',ArrayX',ArrayXHAT',ArrayXD',ArrayXDHAT'];
	save datfil.txt output  -ascii
	disp 'simulation finished'
elseif STATE==3
	output=[ArrayT',ArrayX',ArrayXHAT',ArrayXD',ArrayXDHAT',ArrayXDD',...
	ArrayXDDHAT'];
	save datfil.txt output  -ascii
	disp 'simulation finished'
elseif STATE==4
	output=[ArrayT',ArrayX',ArrayXHAT',ArrayXD',ArrayXDHAT',ArrayXDD',...
	ArrayXDDHAT',ArrayXDDD',ArrayXDDDHAT'];
	save datfil.txt output  -ascii
	disp 'simulation finished'
else
	output=[ArrayT',ArrayX',ArrayXHAT',ArrayXD',ArrayXDHAT',ArrayXDD',...
	ArrayXDDHAT',ArrayXDDD',ArrayXDDDHAT',ArrayXDDDD',ArrayXDDDDHAT'];
	save datfil.txt output  -ascii
	disp 'simulation finished'
end