c10l3.m

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

HP=.001;
W=1.;
A=1.;
TS=.1;
ORDER=3;
PHIS=0.;
SIGX=1.;
T=0.;
S=0.;
H=.001;
F=zeros(ORDER,ORDER);
PHI=zeros(ORDER,ORDER);
P=zeros(ORDER,ORDER);
IDNP=eye(ORDER);
Q=zeros(ORDER,ORDER);
RMAT(1,1)=SIGX^2;
P(1,1)=SIGX^2;
P(2,2)=2.^2;
P(3,3)=2.^2;
XTH=0.;
XTDH=0.;
WH=2.;
XT=0.;
XTD=A*W;
count=0;
while T<=20.
	XTOLD=XT;
	XTDOLD=XTD;
	XTDD=-W*W*XT;
 	XT=XT+H*XTD;
 	XTD=XTD+H*XTDD;
	T=T+H;
	XTDD=-W*W*XT;
 	XT=.5*(XTOLD+XT+H*XTD);
 	XTD=.5*(XTDOLD+XTD+H*XTDD);
	S=S+H;
	if S>=(TS-.00001)
		S=0.;
		F(1,2)=1.;
		F(2,1)=-WH^2;
		F(2,3)=-2.*WH*XTH;
		PHI(1,1)=1.;
		PHI(1,2)=TS;
		PHI(2,1)=-WH*WH*TS;
		PHI(2,2)=1.;
		PHI(2,3)=-2.*WH*XTH*TS;
		PHI(3,3)=1.;
		Q(2,2)=4.*WH*WH*XTH*XTH*TS*TS*TS*PHIS/3.;
		Q(2,3)=-2.*WH*XTH*TS*TS*PHIS/2.;
		Q(3,2)=Q(2,3);
		Q(3,3)=PHIS*TS;
		HMAT(1,1)=1.;
		HMAT(1,2)=0.;
		HMAT(1,3)=0.;
		PHIT=PHI';
		HT=HMAT';
		PHIP=PHI*P;
		PHIPPHIT=PHIP*PHIT;
		M=PHIPPHIT+Q;
		HM=HMAT*M;
		HMHT=HM*HT;
		HMHTR=HMHT+RMAT;
		HMHTRINV(1,1)=1./HMHTR(1,1);
		MHT=M*HT;
		K=MHT*HMHTRINV;
		KH=K*HMAT;
		IKH=IDNP-KH;
		P=IKH*M;
		XTNOISE=SIGX*randn;
		XTMEAS=XT+XTNOISE;
		[XTB,XTDB]=PROJECT4(T,TS,XTH,XTDH,HP,WH);
		RES=XTMEAS-XTB;
		XTH=XTB+K(1,1)*RES;
		XTDH=XTDB+K(2,1)*RES;
		WH=WH+K(3,1)*RES;
		ERRX=XT-XTH;
		SP11=sqrt(P(1,1));
		ERRXD=XTD-XTDH;
		SP22=sqrt(P(2,2));
		ERRW=W-WH;
		SP33=sqrt(P(3,3));
		SP11P=-SP11;
		SP22P=-SP22;
		SP33P=-SP33;
		count=count+1;
		ArrayT(count)=T;
		ArrayW(count)=W;
		ArrayWH(count)=WH;
		ArrayXT(count)=XT;
		ArrayXTH(count)=XTH;
		ArrayXTD(count)=XTD;
		ArrayXTDH(count)=XTDH;
		ArrayERRX(count)=ERRX;
		ArraySP11(count)=SP11;
		ArraySP11P(count)=SP11P;
		ArrayERRXD(count)=ERRXD;
		ArraySP22(count)=SP22;
		ArraySP22P(count)=SP22P;
		ArrayERRW(count)=ERRW;
		ArraySP33(count)=SP33;
		ArraySP33P(count)=SP33P;
		end
end
figure
plot(ArrayT,ArrayXT,ArrayT,ArrayXTH),grid
xlabel('Time (Sec)')
ylabel('x')
axis([0 20 -2 2])
figure
plot(ArrayT,ArrayXTD,ArrayT,ArrayXTDH),grid
xlabel('Time (Sec)')
ylabel('x Dot')
axis([0 20 -3 3])
figure
plot(ArrayT,ArrayW,ArrayT,ArrayWH),grid
xlabel('Time (Sec)')
ylabel('Frequency (R/S)')
axis([0 20 0 3])
clc
output=[ArrayT',ArrayXT',ArrayXTH',ArrayXTD',ArrayXTDH',ArrayW',ArrayWH'];
save datfil.txt output  -ascii
output=[ArrayT',ArrayERRX',ArraySP11',ArraySP11P',ArrayERRXD',ArraySP22',...
ArraySP22P',ArrayERRW',ArraySP33',ArraySP33P'];
save covfil.txt output  -ascii
disp 'simulation finished'