c10l2.m

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

TS=.1;
A=1.;
W=1.;
PHIS=0.;
SIGX=1.;
ORDER=2;
T=0.;
S=0.;
H=.001;
PHI=zeros(ORDER,ORDER);
P=zeros(ORDER,ORDER);
IDNP=eye(ORDER);
Q=zeros(ORDER,ORDER);
RMAT(1,1)=SIGX^2;
PHIH=0.;
WH=2.;
P(1,1)=0.^2;
P(2,2)=(W-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.;
		PHI(1,1)=1.;
		PHI(1,2)=TS;
		PHI(2,2)=1.;
		Q(1,1)=TS*TS*TS*PHIS/3.;
		Q(1,2)=.5*TS*TS*PHIS;
		Q(2,1)=Q(1,2);
		Q(2,2)=PHIS*TS;
		PHIB=PHIH+WH*TS;
		HMAT(1,1)=cos(PHIB);
		HMAT(1,2)=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;
		RES=XTMEAS-A*sin(PHIB);
		PHIH=PHIB+K(1,1)*RES;
		WH=WH+K(2,1)*RES;
		PHIREAL=W*T;
		ERRPHI=PHIREAL-PHIH;
		SP11=sqrt(P(1,1));
		ERRW=W-WH;
		SP22=sqrt(P(2,2));
		XTH=A*sin(PHIH);
		XTDH=A*WH*cos(PHIH);
		SP11P=-SP11;
		SP22P=-SP22;
		count=count+1;
		ArrayT(count)=T;
		ArrayW(count)=W;
		ArrayWH(count)=WH;
		ArrayERRPHI(count)=ERRPHI;
		ArraySP11(count)=SP11;
		ArraySP11P(count)=SP11P;
		ArrayERRW(count)=ERRW;
		ArraySP22(count)=SP22;
		ArraySP22P(count)=SP22P;
		end
end
figure
plot(ArrayT,ArrayW,ArrayT,ArrayWH),grid
xlabel('Time (Sec)')
ylabel('Frequency (R/S)')
axis([0 20 0 2])
clc
output=[ArrayT',ArrayW',ArrayWH'];
save datfil.txt output  -ascii
output=[ArrayT',ArrayERRPHI',ArraySP11',ArraySP11P',ArrayERRW',ArraySP22',...
ArraySP22P'];
save covfil.txt output  -ascii
disp 'simulation finished'