project3.m

来自「这是zarchan书的fundamentals of kalman filter」· M 代码 · 共 22 行

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;