project3.m

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

function [THH,THDH,RH,RDH]=project3(TP,TS,THP,THDP,RP,RDP,HP)
T=0.;
G=32.2;
TH=THP;
THD=THDP;
R=RP;
RD=RDP;
H=HP;
while T<=(TS-.0001)
 	THDD=(-G*R*cos(TH)-2.*THD*R*RD)/R^2;
	RDD=(R*R*THD*THD-G*R*sin(TH))/R;
	THD=THD+H*THDD;
	TH=TH+H*THD;
	RD=RD+H*RDD;
	R=R+H*RD;
	T=T+H;
end
RH=R;
RDH=RD;
THH=TH;
THDH=THD;