c9l2.m

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

VT=3000.;
GAMDEG=45.;
G=32.2;
TS=1.;
XT=0.;
YT=0.;
XTD=VT*cos(GAMDEG/57.3);
YTD=VT*sin(GAMDEG/57.3);
XR=100000.;
YR=0.;
T=0.;
S=0.;
H=.001;
THET=atan2((YT-YR),(XT-XR));
RT=sqrt((XT-XR)^2+(YT-YR)^2);
THETD=((XT-XR)*YTD-(YT-YR)*XTD)/RT^2;
RTD=((XT-XR)*XTD+(YT-YR)*YTD)/RT;
count=0;
while YT>=0.
	XTOLD=XT;
	XTDOLD=XTD;
	YTOLD=YT;
	YTDOLD=YTD;
	THETOLD=THET;
	THETDOLD=THETD;
	RTOLD=RT;
	RTDOLD=RTD;
	XTDD=0.;
 	YTDD=-G;
 	THETDD=(-G*cos(THET)-2.*RTD*THETD)/RT;
 	RTDD=((RT*THETD)^2-G*RT*sin(THET))/RT;
 	XT=XT+H*XTD;
 	XTD=XTD+H*XTDD;
	YT=YT+H*YTD;
	YTD=YTD+H*YTDD;
	THET=THET+H*THETD;
	THETD=THETD+H*THETDD;
	RT=RT+H*RTD;
	RTD=RTD+H*RTDD;
	T=T+H;
	XTDD=0.;
 	YTDD=-G;
 	THETDD=(-G*cos(THET)-2.*RTD*THETD)/RT;
 	RTDD=((RT*THETD)^2-G*RT*sin(THET))/RT;
 	XT=.5*(XTOLD+XT+H*XTD);
 	XTD=.5*(XTDOLD+XTD+H*XTDD);
	YT=.5*(YTOLD+YT+H*YTD);
	YTD=.5*(YTDOLD+YTD+H*YTDD);
	THET=.5*(THETOLD+THET+H*THETD);
	THETD=.5*(THETDOLD+THETD+H*THETDD);
	RT=.5*(RTOLD+RT+H*RTD);
	RTD=.5*(RTDOLD+RTD+H*RTDD);
	S=S+H;
	if S>=(TS-.00001)
		S=0.;
		XTH=RT*cos(THET)+XR;
		YTH=RT*sin(THET)+YR;
		XTDH=RTD*cos(THET)-RT*sin(THET)*THETD;
		YTDH=RTD*sin(THET)+RT*cos(THET)*THETD;
		count=count+1;
		ArrayT(count)=T;
		ArrayXT(count)=XT;
		ArrayXTH(count)=XTH;
		ArrayYT(count)=YT;
		ArrayYTH(count)=YTH;
		ArrayXTD(count)=XTD;
		ArrayXTDH(count)=XTDH;
		ArrayYTD(count)=YTD;
		ArrayYTDH(count)=YTDH;
	end
end
figure
plot(ArrayXT,ArrayYT,ArrayXTH,ArrayYTH),grid
xlabel('Downrange (Ft)')
ylabel('Altitude (Ft)')
axis([0 300000 0 80000])
figure
plot(ArrayT,ArrayXTD,ArrayT,ArrayXTDH,ArrayT,ArrayYTD,ArrayT,ArrayYTDH),grid
title('Velocity')
xlabel('Time (Sec)')
ylabel('Actual and Estimated Velocity (Ft/Sec)')
axis([0 140 -2200 2200])
clc
output=[ArrayT',ArrayXT',ArrayXTH',ArrayXTD',ArrayXTDH',ArrayYT',ArrayYTH',...
ArrayYTD',ArrayYTDH'];
save datfil.txt output  -ascii
disp 'simulation finished'