c9l3.m

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

SIGTH=.01;
SIGR=100.;
TS=1.;
G=32.2;
VT=3000.;
GAMDEG=45.;
XT=0.;
YT=0.;
XTD=VT*cos(GAMDEG/57.3);
YTD=VT*sin(GAMDEG/57.3);
XR=100000.;
YR=0.;
XTH=XT+1000.;
YTH=YT-1000.;
XTDH=XTD-100.;
YTDH=YTD+100.;
TH=atan2((YT-YR),(XT-XR)+.001);
R=sqrt((XT-XR)^2+(YT-YR)^2);
THD=((XT-XR)*YTD-(YT-YR)*XTD)/R^2;
RD=((XT-XR)*XTD+(YT-YR)*YTD)/R;
THH=atan2((YTH-YR),(XTH-XR)+.001);
RH=sqrt((XTH-XR)^2+(YTH-YR)^2);
THDH=((XTH-XR)*YTDH-(YTH-YR)*XTDH)/RH^2;
RDH=((XTH-XR)*XTDH+(YTH-YR)*YTDH)/RH;
ORDER=4;
TF=100.;
T=0.;
S=0.;
H=.001;
HP=.001;
PHI=zeros(ORDER,ORDER);
P=zeros(ORDER,ORDER);
IDNP=eye(ORDER);
Q=zeros(ORDER,ORDER);
FTS=zeros(ORDER,ORDER);	
A=zeros(ORDER,ORDER);
P(1,1)=(TH-THH)^2;
P(2,2)=(THD-THDH)^2;
P(3,3)=(R-RH)^2;
P(4,4)=(RD-RDH)^2;
RMAT(1,1)=SIGTH^2;
RMAT(1,2)=0.;
RMAT(2,1)=0.;
RMAT(2,2)=SIGR^2;
HMAT(1,1)=1.;
HMAT(1,2)=0.;
HMAT(1,3)=0.;
HMAT(1,4)=0.;
HMAT(2,1)=0.;
HMAT(2,2)=0.;
HMAT(2,3)=1.;
HMAT(2,4)=0.;
count=0;
while YT>=0.
	THOLD=TH;
	THDOLD=THD;
	ROLD=R;
	RDOLD=RD;
 	THDD=(-G*R*cos(TH)-2.*THD*R*RD)/R^2;
	RDD=(R*R*THD*THD-G*R*sin(TH))/R;
	TH=TH+H*THD;
	THD=THD+H*THDD;
	R=R+H*RD;
	RD=RD+H*RDD;
 	T=T+H;
	THDD=(-G*R*cos(TH)-2.*THD*R*RD)/R^2;
	RDD=(R*R*THD*THD-G*R*sin(TH))/R;
	TH=.5*(THOLD+TH+H*THD);
	THD=.5*(THDOLD+THD+H*THDD);
	R=.5*(ROLD+R+H*RD);
	RD=.5*(RDOLD+RD+H*RDD);
 	S=S+H;
	if S>=(TS-.00001)
		S=0.;
		FTS(1,2)=1.*TS;
		FTS(2,1)=G*sin(THH)*TS/RH;
		FTS(2,2)=-2.*RDH*TS/RH;
		FTS(2,3)=(G*cos(THH)+2.*THDH*RDH)*TS/RH^2;
		FTS(2,4)=-2.*THDH*TS/RH;
		FTS(3,4)=1.*TS;
		FTS(4,1)=-G*cos(THH)*TS;
		FTS(4,2)=2.*RH*THDH*TS;
		FTS(4,3)=(THDH^2)*TS;
		PHI=FTS+IDNP;
		HT=HMAT';
		PHIT=PHI';
		PHIP=PHI*P;
		PHIPPHIT=PHIP*PHIT;
		M=PHIPPHIT+Q;
		HM=HMAT*M;
		HMHT=HM*HT;
		HMHTR=HMHT+RMAT;
		HMHTRINV=inv(HMHTR);
		MHT=M*HT;
		GAIN=MHT*HMHTRINV;
		KH=GAIN*HMAT;
		IKH=IDNP-KH;
		P=IKH*M;
		THNOISE=SIGTH*randn;
		RNOISE=SIGR*randn;
 		[THB,THDB,RB,RDB]=PROJECT3(T,TS,THH,THDH,RH,RDH,HP);
		RES1=TH+THNOISE-THB;
		RES2=R+RNOISE-RB;
		THH=THB+GAIN(1,1)*RES1+GAIN(1,2)*RES2;
		THDH=THDB+GAIN(2,1)*RES1+GAIN(2,2)*RES2;
		RH=RB+GAIN(3,1)*RES1+GAIN(3,2)*RES2;
		RDH=RDB+GAIN(4,1)*RES1+GAIN(4,2)*RES2;
		ERRTH=TH-THH;
		SP11=sqrt(P(1,1));
		ERRTHD=THD-THDH;
		SP22=sqrt(P(2,2));
		ERRR=R-RH;
		SP33=sqrt(P(3,3));
		ERRRD=RD-RDH;
		SP44=sqrt(P(4,4));
		XT=R*cos(TH)+XR;
		YT=R*sin(TH)+YR;
		XTD=RD*cos(TH)-R*THD*sin(TH);
		YTD=RD*sin(TH)+R*THD*cos(TH);
		XTH=RH*cos(THH)+XR;
		YTH=RH*sin(THH)+YR;
		XTDH=RDH*cos(THH)-RH*THDH*sin(THH);
		YTDH=RDH*sin(THH)+RH*THDH*cos(THH);
		A(1,1)=-RH*sin(THH);
		A(1,3)=cos(THH);
		A(2,1)=-RDH*sin(THH)-RH*THDH*cos(THH);
		A(2,2)=-RH*sin(THH);
		A(2,3)=-THDH*sin(THH);
		A(2,4)=cos(THH);
		A(3,1)=RH*cos(THH);
		A(3,3)=sin(THH);
		A(4,1)=RDH*cos(THH)-RH*sin(THH)*THDH;
		A(4,2)=RH*cos(THH);
		A(4,3)=THDH*cos(THH);
		A(4,4)=sin(THH);
		AT=A';
		AP=A*P;
		PNEW=AP*AT;
		ERRXT=XT-XTH;
		SP11P=sqrt(PNEW(1,1));
		ERRXTD=XTD-XTDH;
		SP22P=sqrt(PNEW(2,2));
		ERRYT=YT-YTH;
		SP33P=sqrt(PNEW(3,3));
		ERRYTD=YTD-YTDH;
		SP44P=sqrt(PNEW(4,4));
		SP11PP=-SP11P;
		SP22PP=-SP22P;
		SP33PP=-SP33P;
		SP44PP=-SP44P;
		count=count+1;
		ArrayT(count)=T;
		ArrayERRXT(count)=ERRXT;
		ArrayERRXTD(count)=ERRXTD;
		ArrayERRYT(count)=ERRYT;
		ArrayERRYTD(count)=ERRYTD;
		ArraySP11P(count)=SP11P;
		ArraySP11PP(count)=SP11PP;
		ArraySP22P(count)=SP22P;
		ArraySP22PP(count)=SP22PP;
		ArraySP33P(count)=SP33P;
		ArraySP33PP(count)=SP33PP;
		ArraySP44P(count)=SP44P;
		ArraySP44PP(count)=SP44PP;
     	end
end
figure
plot(ArrayT,ArrayERRXT,ArrayT,ArraySP11P,ArrayT,ArraySP11PP),grid
xlabel('Time (Sec)')
ylabel('Error in Estimate of Downrange (Ft)')
axis([0 140 -200 200])
figure
plot(ArrayT,ArrayERRXTD,ArrayT,ArraySP22P,ArrayT,ArraySP22PP),grid
xlabel('Time (Sec)')
ylabel('Error in Estimate of Downrange Velocity (Ft/Sec)')
axis([0 140 -20 20])
figure
plot(ArrayT,ArrayERRYT,ArrayT,ArraySP33P,ArrayT,ArraySP33PP),grid
xlabel('Time (Sec)')
ylabel('Error in Estimate of Altitude (Ft)')
axis([0 140 -600 600])
figure
plot(ArrayT,ArrayERRYTD,ArrayT,ArraySP44P,ArrayT,ArraySP44PP),grid
xlabel('Time (Sec)')
ylabel('Error in Estimate of Altitude Velocity (Ft/Sec)')
axis([0 140 -20 20])
clc
output=[ArrayT',ArrayERRXT',ArraySP11P',ArraySP11PP',ArrayERRXTD',ArraySP22P',...
ArraySP22PP',ArrayERRYT',ArraySP33P',ArraySP33PP',ArrayERRYTD',ArraySP44P',...
ArraySP44PP'];
save covfil.txt output  -ascii
disp 'simulation finished'