c12l4.m

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

TF=100.;
SIGNOISE=300.;
X=0.;
Y=0.;
XH=1000.;
YH=2000.;
XR1=1000000.;
YR1=20000.*3280.;
XR2=50000000.;
YR2=20000.*3280.;
XR3=1000000000.;
YR3=20000.*3280.;
BIAS=10000.;
BIASH=0.;
PHIS=0.;
ORDER=3;
TS=1.;
T=0.;
S=0.;
H=.01;
PHI=zeros(ORDER,ORDER);
P=zeros(ORDER,ORDER);
IDNP=eye(ORDER);
Q=zeros(ORDER,ORDER);
RMAT=zeros(3,3);
Q(3,3)=PHIS*TS;
P(1,1)=1000.^2;
P(2,2)=2000.^2;
P(3,3)=(BIAS-BIASH)^2;
RMAT(1,1)=SIGNOISE^2;
RMAT(2,2)=SIGNOISE^2;
RMAT(3,3)=SIGNOISE^2;
count=0;
while T<=TF
	XR1OLD=XR1;
	XR2OLD=XR2;
	XR3OLD=XR3;
 	XR1D=-14600.;
	XR2D=-14600.;
	XR3D=-14600.;
 	XR1=XR1+H*XR1D;
	XR2=XR2+H*XR2D;
	XR3=XR3+H*XR3D;
 	T=T+H;
	XR1D=-14600.;
	XR2D=-14600.;
	XR3D=-14600.;
 	XR1=.5*(XR1OLD+XR1+H*XR1D);
	XR2=.5*(XR2OLD+XR2+H*XR2D);
	XR3=.5*(XR3OLD+XR3+H*XR3D);
 	S=S+H;
	if S>=(TS-.00001)
		S=0.;
		R1H=sqrt((XR1-XH)^2+(YR1-YH)^2)+BIASH;
		R2H=sqrt((XR2-XH)^2+(YR2-YH)^2)+BIASH;
		R3H=sqrt((XR3-XH)^2+(YR3-YH)^2)+BIASH;
		HMAT(1,1)=-(XR1-XH)/sqrt((XR1-XH)^2+(YR1-YH)^2);
		HMAT(1,2)=-(YR1-YH)/sqrt((XR1-XH)^2+(YR1-YH)^2);
		HMAT(1,3)=1.;
		HMAT(2,1)=-(XR2-XH)/sqrt((XR2-XH)^2+(YR2-YH)^2);
		HMAT(2,2)=-(YR2-YH)/sqrt((XR2-XH)^2+(YR2-YH)^2);
		HMAT(2,3)=1.;
		HMAT(3,1)=-(XR3-XH)/sqrt((XR3-XH)^2+(YR3-YH)^2);
		HMAT(3,2)=-(YR3-YH)/sqrt((XR3-XH)^2+(YR3-YH)^2);
		HMAT(3,3)=1.;
		HT=HMAT';
		PHI(1,1)=1.;
		PHI(2,2)=1.;
		PHI(3,3)=1.;
 		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;
 		R1NOISE=SIGNOISE*randn;
 		R2NOISE=SIGNOISE*randn;
 		R3NOISE=SIGNOISE*randn;
		R1=sqrt((XR1-X)^2+(YR1-Y)^2);
		R2=sqrt((XR2-X)^2+(YR2-Y)^2);
		R3=sqrt((XR3-X)^2+(YR3-Y)^2);
		RES1=R1+R1NOISE+BIAS-R1H;
		RES2=R2+R2NOISE+BIAS-R2H;
		RES3=R3+R3NOISE+BIAS-R3H;
		XH=XH+GAIN(1,1)*RES1+GAIN(1,2)*RES2+GAIN(1,3)*RES3;
		YH=YH+GAIN(2,1)*RES1+GAIN(2,2)*RES2+GAIN(2,3)*RES3;
		BIASH=BIASH+GAIN(3,1)*RES1+GAIN(3,2)*RES2+GAIN(3,3)*RES3;
		ERRX=X-XH;
		SP11=sqrt(P(1,1));
		ERRY=Y-YH;
		SP22=sqrt(P(2,2));
		ERRBIAS=BIAS-BIASH;
		SP33=sqrt(P(3,3));
		R1T=sqrt((XR1-X)^2+(YR1-Y)^2);
		R2T=sqrt((XR2-X)^2+(YR2-Y)^2);
		TEMP=(XR1-X)*(XR2-X)+(YR1-Y)*(YR2-Y);
		TH1=atan2(sqrt((R1T*R2T)^2-TEMP^2),TEMP);
		THET1=57.3*TH1;
		R3T=sqrt((XR3-X)^2+(YR3-Y)^2);
		TEMP3=(XR2-X)*(XR3-X)+(YR2-Y)*(YR3-Y);
		TH2=atan2(sqrt((R2T*R3T)^2-TEMP3^2),TEMP3);
		THET2=57.3*TH2;
		SP11P=-SP11;
		SP22P=-SP22;
		SP33P=-SP33;
		count=count+1;
		ArrayT(count)=T;
		ArrayX(count)=X;
		ArrayXH(count)=XH;
		ArrayY(count)=Y;
		ArrayYH(count)=YH;
		ArrayBIAS(count)=BIAS;
		ArrayBIASH(count)=BIASH;
		ArrayERRX(count)=ERRX;
		ArraySP11(count)=SP11;
		ArraySP11P(count)=SP11P;
		ArrayERRY(count)=ERRY;
		ArraySP22(count)=SP22;
		ArraySP22P(count)=SP22P;
		ArrayERRBIAS(count)=ERRBIAS;
		ArraySP33(count)=SP33;
		ArraySP33P(count)=SP33P;
		end
end
figure
plot(ArrayT,ArrayBIAS,ArrayT,ArrayBIASH),grid
xlabel('Time (Sec)')
ylabel('Bias (Ft)')
axis([0 100 9000 11000])
figure
plot(ArrayT,ArrayX,ArrayT,ArrayXH),grid
xlabel('Time (Sec)')
ylabel('Receiver Downrange (Ft)')
axis([0 100 -200 800])
figure
plot(ArrayT,ArrayY,ArrayT,ArrayYH),grid
xlabel('Time (Sec)')
ylabel('Receiver Altitude (Ft)')
axis([0 100 -200 1200])
figure
plot(ArrayT,ArrayERRX,ArrayT,ArraySP11,ArrayT,ArraySP11P),grid
xlabel('Time (Sec)')
ylabel('Error in Estimate of Downrange (Ft)')
axis([0 100 -700 700])
figure
plot(ArrayT,ArrayERRY,ArrayT,ArraySP22,ArrayT,ArraySP22P),grid
xlabel('Time (Sec)')
ylabel('Error in Estimate of Altitude (Ft)')
axis([0 100 -500 500])
figure
plot(ArrayT,ArrayERRBIAS,ArrayT,ArraySP33,ArrayT,ArraySP33P),grid
xlabel('Time (Sec)')
ylabel('Error in Estimate of Bias (Ft)')
axis([0 100 -500 500])
clc
output=[ArrayT',ArrayX',ArrayXH',ArrayY',ArrayYH',ArrayBIAS',ArrayBIASH'];
save datfil.txt output  -ascii
output=[ArrayT',ArrayERRX',ArraySP11',ArraySP11P',ArrayERRY',ArraySP22',...
ArraySP22P',ArrayERRBIAS',ArraySP33',ArraySP33P'];
save covfil.txt output  -ascii
disp 'simulation finished'