project2.m

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

function [XH,XDH,XDDH]=project2(TP,TS,XP,XDP,BETA,HP)
T=0.;
X=XP;
XD=XDP;
H=HP;
while T<=(TS-.0001)
	XDD=.0034*32.2*XD*XD*exp(-X/22000.)/(2.*BETA)-32.2;
	XD=XD+H*XDD;
	X=X+H*XD;
	T=T+H;
end
XH=X;
XDH=XD;
XDDH=XDD;