c10l1.m

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

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