c9l4.m

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

TS=1.;
ORDER=2;
PHIS=0.;
SIGTH=.01;
SIGR=100.;
VT=3000.;
GAMDEG=45.;
G=32.2;
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;
PHI=zeros(ORDER,ORDER);
P=zeros(ORDER,ORDER);
IDNP=eye(ORDER);
Q=zeros(ORDER,ORDER);	
PHI(1,1)=1.;
PHI(1,2)=TS;
PHI(2,2)=1.;
HMAT(1,1)=1.;
HMAT(1,2)=0.;
PHIT=PHI';
HT=HMAT';
Q(1,1)=PHIS*TS*TS*TS/3.;
Q(1,2)=PHIS*TS*TS/2.;
Q(2,1)=Q(1,2);
Q(2,2)=PHIS*TS;
P(1,1)=1000.^2;
P(2,2)=100.^2;
PY(1,1)=1000.^2;
PY(2,2)=100.^2;
XTH=XT+1000.;
XTDH=XTD-100.;
YTH=YT-1000.;
YTDH=YTD-100.;
count=0;
while YT>=0.
	XTOLD=XT;
	XTDOLD=XTD;
	YTOLD=YT;
	YTDOLD=YTD;
	XTDD=0.;
 	YTDD=-G;
 	XT=XT+H*XTD;
 	XTD=XTD+H*XTDD;
	YT=YT+H*YTD;
	YTD=YTD+H*YTDD;
	T=T+H;
	XTDD=0.;
 	YTDD=-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);
	S=S+H;
	if S>=(TS-.00001)
		S=0.;
		THETH=atan2((YTH-YR),(XTH-XR));
		RTH=sqrt((XTH-XR)^2+(YTH-YR)^2);
		RMAT(1,1)=(cos(THETH)*SIGR)^2+(RTH*sin(THETH)*SIGTH)^2;
		PHIP=PHI*P;
		PHIPPHIT=PHIP*PHIT;
		M=PHIPPHIT+Q;
		HM=HMAT*M;
		HMHT=HM*HT;
		HMHTR=HMHT+RMAT;
		HMHTRINV(1,1)=1./HMHTR(1,1);
		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);
		THETMEAS=THET+THETNOISE;
		RTMEAS=RT+RTNOISE;
		XTMEAS=RTMEAS*cos(THETMEAS)+XR;
		RES1=XTMEAS-XTH-TS*XTDH;
		XTH=XTH+TS*XTDH+K(1,1)*RES1;
		XTDH=XTDH+K(2,1)*RES1;
		RMATY(1,1)=(sin(THETH)*SIGR)^2+(RTH*cos(THETH)*SIGTH)^2;
		PHIPY=PHI*PY;
		PHIPPHITY=PHIPY*PHIT;
		MY=PHIPPHITY+Q;
		HMY=HMAT*MY;
		HMHTY=HMY*HT;
		HMHTRY=HMHTY+RMATY;
		HMHTRINVY(1,1)=1./HMHTRY(1,1);
		MHTY=MY*HT;
		KY=MHTY*HMHTRINVY;
		KHY=KY*HMAT;
		IKHY=IDNP-KHY;
		PY=IKHY*MY;
		YTMEAS=RTMEAS*sin(THETMEAS)+YR;
		RES2=YTMEAS-YTH-TS*YTDH+.5*TS*TS*G;
		YTH=YTH+TS*YTDH-.5*TS*TS*G+KY(1,1)*RES2;
		YTDH=YTDH-TS*G+KY(2,1)*RES2;
		ERRX=XT-XTH;
		SP11=sqrt(P(1,1));
		ERRXD=XTD-XTDH;
		SP22=sqrt(P(2,2));
		ERRY=YT-YTH;
		SP11Y=sqrt(PY(1,1));
		ERRYD=YTD-YTDH;
		SP22Y=sqrt(PY(2,2));
		SP11P=-SP11;
		SP22P=-SP22;
		SP11YP=-SP11Y;
		SP22YP=-SP22Y;
		count=count+1;
		ArrayT(count)=T;
		ArrayERRX(count)=ERRX;
		ArrayERRXD(count)=ERRXD;
		ArrayERRY(count)=ERRY;
		ArrayERRYD(count)=ERRYD;
		ArraySP11(count)=SP11;
		ArraySP11P(count)=SP11P;
		ArraySP22(count)=SP22;
		ArraySP22P(count)=SP22P;
		ArraySP11Y(count)=SP11Y;
		ArraySP11YP(count)=SP11YP;
		ArraySP22Y(count)=SP22Y;
		ArraySP22YP(count)=SP22YP;
		end
end
figure
plot(ArrayT,ArrayERRX,ArrayT,ArraySP11,ArrayT,ArraySP11P),grid
xlabel('Time (Sec)')
ylabel('Error in Estimate of Downrange (Ft)')
axis([0 140 -250 250])
figure
plot(ArrayT,ArrayERRXD,ArrayT,ArraySP22,ArrayT,ArraySP22P),grid
xlabel('Time (Sec)')
ylabel('Error in Estimate of Downrange Velocity (Ft/Sec)')
axis([0 140 -20 20])
figure
plot(ArrayT,ArrayERRY,ArrayT,ArraySP11Y,ArrayT,ArraySP11YP),grid
xlabel('Time (Sec)')
ylabel('Error in Estimate of Altitude (Ft)')
axis([0 140 -700 700])
figure
plot(ArrayT,ArrayERRYD,ArrayT,ArraySP22Y,ArrayT,ArraySP22YP),grid
xlabel('Time (Sec)')
ylabel('Error in Estimate of Altitude Velocity (Ft/Sec)')
axis([0 140 -20 20])
clc
output=[ArrayT',ArrayERRX',ArraySP11',ArraySP11P',ArrayERRXD',ArraySP22',...
ArraySP22P',ArrayERRY',ArraySP11Y',ArraySP11YP',ArrayERRYD',ArraySP22Y',...
ArraySP22YP'];
save covfil.txt output  -ascii
disp 'simulation finished'