c1l11.m

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

clear
count=0;
TAU=.2;
PHI=1.;
T=0.;
H=.01;
SIG=sqrt(PHI/H);
Y=0.;
while T<=4.999
 	X=SIG*randn;
 	YOLD=Y;
	YD=(X-Y)/TAU;
 	Y=Y+H*YD;
	T=T+H;
	YD=(X-Y)/TAU;
 	Y=(YOLD+Y)/2.+.5*H*YD;
	SIGPLUS=sqrt(PHI*(1.-exp(-2.*T/TAU))/(2.*TAU));
	SIGMINUS=-SIGPLUS;
	count=count+1;
      	ArrayT(count)=T;
      	ArrayY(count)=Y;
      	ArraySIGPLUS(count)=SIGPLUS;
      	ArraySIGMINUS(count)=SIGMINUS;
end
figure
plot(ArrayT,ArrayY,ArrayT,ArraySIGPLUS,ArrayT,ArraySIGMINUS),grid
xlabel('Time (Sec)')
ylabel('y ')
axis([0 5 -5 5])
clc
output=[ArrayT',ArrayY',ArraySIGPLUS',ArraySIGMINUS'];
save datfil.txt output /ascii
disp 'simulation finished'