c11l7.m

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

SIGNOISE=300.;
PHIS=0.;
X=0.;
Y=0.;
XH=1000.;
YH=2000.;
XDH=0.;
YDH=0.;
XR1=1000000.;
YR1=20000.*3280.;
ORDER=4;
TS=1.;
TF=1000.;
T=0.;
S=0.;
H=.01;
PHI=zeros(ORDER,ORDER);
P=zeros(ORDER,ORDER);
IDNP=eye(ORDER);
Q=zeros(ORDER,ORDER);	
P(1,1)=1000.^2;
P(2,2)=100.^2;
P(3,3)=2000.^2;
P(4,4)=100.^2;
RMAT(1,1)=SIGNOISE^2;
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)=PHIS*TS3/3.;
Q(3,4)=PHIS*TS2/2.;
Q(4,3)=Q(3,4);
Q(4,4)=PHIS*TS;
count=0;
while T<=TF
	XR1OLD=XR1;
	XOLD=X;
	YOLD=Y;
 	XR1D=-14600.;
	XD=100.;
	YD=0.;
 	XR1=XR1+H*XR1D;
	X=X+H*XD;
	Y=Y+H*YD;
 	T=T+H;
	XR1D=-14600.;
	XD=100.;
	YD=0.;
 	XR1=.5*(XR1OLD+XR1+H*XR1D);
	X=.5*(XOLD+X+H*XD);
	Y=.5*(YOLD+Y+H*YD);
 	S=S+H;
	if S>=(TS-.00001)
		S=0.;
		XB=XH+XDH*TS;
		YB=YH+YDH*TS;
		R1B=sqrt((XR1-XB)^2+(YR1-YB)^2);
		HMAT(1,1)=-(XR1-XB)/R1B;
		HMAT(1,2)=0.;
		HMAT(1,3)=-(YR1-YB)/R1B;
		HMAT(1,4)=0.;
		HT=HMAT';
		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';
 		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;
 		GAIN=MHT*HMHTRINV;
		KH=GAIN*HMAT;
 		IKH=IDNP-KH;
 		P=IKH*M;
 		R1NOISE=SIGNOISE*randn;
		R1=sqrt((XR1-X)^2+(YR1-Y)^2);
		RES1=R1+R1NOISE-R1B;
		XH=XB+GAIN(1,1)*RES1;
		XDH=XDH+GAIN(2,1)*RES1;
		YH=YB+GAIN(3,1)*RES1;
		YDH=YDH+GAIN(4,1)*RES1;
		ERRX=X-XH;
		SP11=sqrt(P(1,1));
		ERRXD=XD-XDH;
		SP22=sqrt(P(2,2));
		ERRY=Y-YH;
		SP33=sqrt(P(3,3));
		ERRYD=YD-YDH;
		SP44=sqrt(P(4,4));
		SP11P=-SP11;
		SP22P=-SP22;
		SP33P=-SP33;
		SP44P=-SP44;
		count=count+1;
		ArrayT(count)=T;
		ArrayX(count)=X;
		ArrayXH(count)=XH;
		ArrayXD(count)=XD;
		ArrayXDH(count)=XDH;
		ArrayY(count)=Y;
		ArrayYH(count)=YH;
		ArrayYD(count)=YD;
		ArrayYDH(count)=YDH;
		ArrayERRX(count)=ERRX;
		ArraySP11(count)=SP11;
		ArraySP11P(count)=SP11P;
		ArrayERRXD(count)=ERRXD;
		ArraySP22(count)=SP22;
		ArraySP22P(count)=SP22P;
		ArrayERRY(count)=ERRY;
		ArraySP33(count)=SP33;
		ArraySP33P(count)=SP33P;
		ArrayERRYD(count)=ERRYD;
		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 Downrange (Ft)')
axis([0 1000 -11000 11000])
figure
plot(ArrayT,ArrayERRXD,ArrayT,ArraySP22,ArrayT,ArraySP22P),grid
xlabel('Time (Sec)')
ylabel('Error in Estimate of Downrange Velocity (Ft/Sec)')
axis([0 1000 -20 20])
figure
plot(ArrayT,ArrayERRY,ArrayT,ArraySP33,ArrayT,ArraySP33P),grid
xlabel('Time (Sec)')
ylabel('Error in Estimate of Altitude (Ft)')
axis([0 1000 -500 500])
figure
plot(ArrayT,ArrayERRYD,ArrayT,ArraySP44,ArrayT,ArraySP44P),grid
title('Receiver Altitude Velocity')
xlabel('Time (Sec)')
ylabel('Error in Estimate of Altitude Velocity (Ft/Sec)')
axis([0 1000 -20 20])
clc
output=[ArrayT',ArrayX',ArrayXH',ArrayXD',ArrayXDH',ArrayY',ArrayYH',...
ArrayYD',ArrayYDH'];
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'