c14l1.m

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

ORDER=2;
PHIS=0.;
W=1;
A=1;
TS=.1;
XH=0.;
XDH=0.;
SIGNOISE=1.;
PHI=zeros(ORDER,ORDER);
P=zeros(ORDER,ORDER);
IDNP=eye(ORDER);
Q=zeros(ORDER,ORDER);
RMAT(1,1)=SIGNOISE^2;
P(1,1)=99999999999.;
P(2,2)=99999999999.;
PHI(1,1)=cos(W*TS);
PHI(1,2)=sin(W*TS)/W;
PHI(2,1)=-W*sin(W*TS);
PHI(2,2)=cos(W*TS);
HMAT(1,1)=1.;
HMAT(1,2)=0.;
PHIT=PHI';
HT=HMAT';
Q(1,1)=PHIS*TS^3/3;
Q(1,2)=PHIS*TS*TS/2;
Q(2,1)=Q(1,2);
Q(2,2)=PHIS*TS;
count=0;
for T=0:TS:20
	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;
	XNOISE=SIGNOISE*randn;
	X=A*sin(W*T);
	XD=A*W*cos(W*T);
	XS=X+XNOISE;
	XHOLD=XH;
	RES=XS-XH*cos(W*TS)-sin(W*TS)*XDH/W;
	XH=cos(W*TS)*XH+XDH*sin(W*TS)/W+K(1,1)*RES;
	XDH=-W*sin(W*TS)*XHOLD+XDH*cos(W*TS)+K(2,1)*RES;
	count=count+1;
	ArrayT(count)=T;
	ArrayX(count)=X;
	ArrayXS(count)=XS;
	ArrayXH(count)=XH;
	ArrayXD(count)=XD;
	ArrayXDH(count)=XDH;
end
figure
plot(ArrayT,ArrayX,ArrayT,ArrayXS),grid
xlabel('Time (Sec)')
ylabel('Measurement and Signal')
axis([0 20 -4 4])
figure
plot(ArrayT,ArrayX,ArrayT,ArrayXH),grid
xlabel('Time (Sec)')
ylabel('x')
axis([0 20 -2 2])
clc
output=[ArrayT',ArrayX',ArrayXS',ArrayXH',ArrayXD',ArrayXDH'];
save datfil.txt output  -ascii
disp 'simulation finished'