c13l3.m

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

TS=1.;
VT=3000.;
VTERR=0.;
GAMDEG=45.;
GAMDEGERR=0.;
VTNOM=VT+VTERR;
GAMDEGNOM=GAMDEG+GAMDEGERR;
G=32.2;
XT=0.;
YT=0.;
XTD=VT*cos(GAMDEG/57.3);
YTD=VT*sin(GAMDEG/57.3);
XTNOM=XT;
YTNOM=YT;
XTDNOM=VTNOM*cos(GAMDEGNOM/57.3);
YTDNOM=VTNOM*sin(GAMDEGNOM/57.3);
XR=100000.;
YR=0.;
T=0.;
S=0.;
H=.001;
count=0;
while YT>=0.
	XTOLD=XT;
	XTDOLD=XTD;
	YTOLD=YT;
	YTDOLD=YTD;
	XTNOMOLD=XTNOM;
	XTDNOMOLD=XTDNOM;
	YTNOMOLD=YTNOM;
	YTDNOMOLD=YTDNOM;
	XTDD=0.;
 	YTDD=-G;
	XTDDNOM=0.;
 	YTDDNOM=-G;
 	XT=XT+H*XTD;
 	XTD=XTD+H*XTDD;
	YT=YT+H*YTD;
	YTD=YTD+H*YTDD;
	XTNOM=XTNOM+H*XTDNOM;
 	XTDNOM=XTDNOM+H*XTDDNOM;
	YTNOM=YTNOM+H*YTDNOM;
	YTDNOM=YTDNOM+H*YTDDNOM;
	T=T+H;
	XTDD=0.;
 	YTDD=-G;
	XTDDNOM=0.;
 	YTDDNOM=-G;
 	XT=.5*(XTOLD+XT+H*XTD);
 	XTD=.5*(XTDOLD+XTD+H*XTDD);
	YT=.5*(YTOLD+YT+H*YTD);
	YTD=.5*(YTDOLD+YTD+H*YTDD);
 	XTNOM=.5*(XTNOMOLD+XTNOM+H*XTDNOM);
 	XTDNOM=.5*(XTDNOMOLD+XTDNOM+H*XTDDNOM);
	YTNOM=.5*(YTNOMOLD+YTNOM+H*YTDNOM);
	YTDNOM=.5*(YTDNOMOLD+YTDNOM+H*YTDDNOM);
	S=S+H;
	if S>=(TS-.00001)
		S=0.;
		RTNOM=sqrt((XTNOM-XR)^2+(YTNOM-YR)^2);
		THET=atan2((YT-YR),(XT-XR));
		RT=sqrt((XT-XR)^2+(YT-YR)^2);
		THETNOM=atan2((YTNOM-YR),(XTNOM-XR));
		DELTHET=57.3*(THET-THETNOM);
		DELRT=RT-RTNOM;
		DELXT=XT-XTNOM;
		DELXTD=XTD-XTDNOM;
		DELYT=YT-YTNOM;
		DELYTD=YTD-YTDNOM;
		count=count+1;
		ArrayT(count)=T;
		ArrayDELXT(count)=DELXT;
		ArrayDELXTD(count)=DELXTD;
		ArrayDELYT(count)=DELYT;
		ArrayDELYTD(count)=DELYTD;
		ArrayDELTHET(count)=DELTHET;
		ArrayDELRT(count)=DELRT;
		end
end
figure
plot(ArrayT,ArrayDELXT),grid
xlabel('Time (Sec)')
ylabel('Error in x (Ft)')
axis([0 140 -20000 5000])
figure
plot(ArrayT,ArrayDELTHET),grid
xlabel('Time (Sec)')
ylabel('Error in Angle (Deg)')
axis([0 140 -7 7])
figure
plot(ArrayT,ArrayDELRT),grid
xlabel('Time (Sec)')
ylabel('Error in Range (Ft)')
axis([0 140 -20000 5000])
clc
output=[ArrayT',ArrayDELXT',ArrayDELXTD',ArrayDELYT',ArrayDELYTD',ArrayDELTHET',...
ArrayDELRT'];
save datfil.txt output  -ascii
disp 'simulation finished'