c13l4.m

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

TS=1.;
ORDER=4;
PHIS=0.;
SIGTH=.01;
SIGR=100.;
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;
XH(1,1)=XT-XTNOM;
XH(2,1)=XTD-XTDNOM;
XH(3,1)=YT-YTNOM;
XH(4,1)=YTD-YTDNOM;
PHI=zeros(ORDER,ORDER);
P=zeros(ORDER,ORDER);
IDNP=eye(ORDER);
Q=zeros(ORDER,ORDER);
TS2=TS*TS;
TS3=TS2*TS;
Q(1,1)=PHIS*TS3/3.;
Q(1,2)=PHIS*TS2/2.;
Q(2,1)=Q(1,2);
Q(2,2)=PHIS*TS;
Q(3,3)=Q(1,1);
Q(3,4)=Q(1,2);
Q(4,3)=Q(3,4);
Q(4,4)=Q(2,2);
PHI(1,1)=1.;
PHI(1,2)=TS;
PHI(2,2)=1.;
PHI(3,3)=1.;
PHI(3,4)=TS;
PHI(4,4)=1.;
PHIT=PHI';
RMAT(1,1)=SIGTH^2;
RMAT(1,2)=0.;
RMAT(2,1)=0.;
RMAT(2,2)=SIGR^2;
P(1,1)=1000.^2;
P(2,2)=100.^2;
P(3,3)=1000.^2;
P(4,4)=100.^2;
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);
		HMAT(1,1)=-(YTNOM-YR)/RTNOM^2;
		HMAT(1,2)=0.;
		HMAT(1,3)=(XTNOM-XR)/RTNOM^2;
		HMAT(1,4)=0.;
		HMAT(2,1)=(XTNOM-XR)/RTNOM;
		HMAT(2,2)=0.;
		HMAT(2,3)=(YTNOM-YR)/RTNOM;
		HMAT(2,4)=0.;
		HT=HMAT';
		PHIP=PHI*P;
		PHIPPHIT=PHIP*PHIT;
		M=PHIPPHIT+Q;
		HM=HMAT*M;
		HMHT=HM*HT;
		HMHTR=HMHT+RMAT;
		HMHTRINV=inv(HMHTR);
		MHT=M*HT;
		K=MHT*HMHTRINV;
		KH=K*HMAT;
		IKH=IDNP-KH;
		P=IKH*M;
		THETNOISE=SIGTH*randn;
		RTNOISE=SIGR*randn;
		THET=atan2((YT-YR),(XT-XR));
		RT=sqrt((XT-XR)^2+(YT-YR)^2);
		THETNOM=atan2((YTNOM-YR),(XTNOM-XR));
		RTNOM=sqrt((XTNOM-XR)^2+(YTNOM-YR)^2);
		DELTHET=THET-THETNOM;
		DELRT=RT-RTNOM;
		MEAS(1,1)=DELTHET+THETNOISE;
		MEAS(2,1)=DELRT+RTNOISE;
		PHIXH=PHI*XH;
		HPHIXH=HMAT*PHIXH;
		RES=MEAS-HPHIXH;
		KRES=K*RES;
		XH=PHIXH+KRES;
		XTH=XTNOM+XH(1,1);
		XTDH=XTDNOM+XH(2,1);
		YTH=YTNOM+XH(3,1);
		YTDH=YTDNOM+XH(4,1);
		ERRX=XT-XTH;
		SP11=sqrt(P(1,1));
		ERRXD=XTD-XTDH;
		SP22=sqrt(P(2,2));
		ERRY=YT-YTH;
		SP33=sqrt(P(3,3));
		ERRYD=YTD-YTDH;
		SP44=sqrt(P(4,4));
		SP11P=-SP11;
		SP22P=-SP22;
		SP33P=-SP33;
		SP44P=-SP44;
		count=count+1;
		ArrayT(count)=T;
		ArrayXT(count)=XT;
		ArrayXTH(count)=XTH;
		ArrayXTD(count)=XTD;
		ArrayXTDH(count)=XTDH;
		ArrayYT(count)=YT;
		ArrayYTH(count)=YTH;
		ArrayYTD(count)=YTD;
		ArrayYTDH(count)=YTDH;
		ArrayERRX(count)=ERRX;
		ArrayERRXD(count)=ERRXD;
		ArrayERRY(count)=ERRY;
		ArrayERRYD(count)=ERRYD;
		ArraySP11(count)=SP11;
		ArraySP11P(count)=SP11P;
		ArraySP22(count)=SP22;
		ArraySP22P(count)=SP22P;
		ArraySP33(count)=SP33;
		ArraySP33P(count)=SP33P;
		ArraySP44(count)=SP44;
		ArraySP44P(count)=SP44P;
		end
end
figure
plot(ArrayT,ArrayERRX,ArrayT,ArraySP11,ArrayT,ArraySP11P),grid
xlabel('Time (Sec)')
ylabel('Error in Estimate of x (Ft)')
axis([0 140 -170 170])
figure
plot(ArrayT,ArrayERRXD,ArrayT,ArraySP22,ArrayT,ArraySP22P),grid
xlabel('Time (Sec)')
ylabel('Error in Estimate of x Dot (Ft/Sec)')
axis([0 140 -20 20])
figure
plot(ArrayT,ArrayERRY,ArrayT,ArraySP33,ArrayT,ArraySP33P),grid
xlabel('Time (Sec)')
ylabel('Error in Estimate of y (Ft)')
axis([0 140 -700 700])
figure
plot(ArrayT,ArrayERRYD,ArrayT,ArraySP44,ArrayT,ArraySP44P),grid
xlabel('Time (Sec)')
ylabel('Error in Estimate of y Dot (Ft/Sec)')
axis([0 140 -20 20])
clc
output=[ArrayT',ArrayXT',ArrayXTH',ArrayXTD',ArrayXTDH',ArrayYT',ArrayYTH'...
,ArrayYTD',ArrayYTDH'];
save datfil.txt output  -ascii
output=[ArrayT',ArrayERRX',ArraySP11',ArraySP11P',ArrayERRXD',ArraySP22',ArraySP22P'...
,ArrayERRY',ArraySP33',ArraySP33P',ArrayERRYD',ArraySP44',ArraySP44P'];
save covfil.txt output  -ascii
disp 'simulation finished'