c12l3.m

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

SIGNOISE=300.;
TF=100.;
X=0.;
Y=0.;
XH=1000.;
YH=2000.;
XR1=1000000.;
YR1=20000.*3280.;
XR2=50000000.;
YR2=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);
Q(3,3)=PHIS*TS;
P(1,1)=1000.^2;
P(2,2)=2000.^2;
P(3,3)=500.^2;
RMAT(1,1)=SIGNOISE^2;
RMAT(1,2)=0.;
RMAT(2,1)=0.;
RMAT(2,2)=SIGNOISE^2;
count=0;
while T<=TF
	XR1OLD=XR1;
	XR2OLD=XR2;
 	XR1D=-14600.;
	XR2D=-14600.;
 	XR1=XR1+H*XR1D;
	XR2=XR2+H*XR2D;
 	T=T+H;
	XR1D=-14600.;
	XR2D=-14600.;
 	XR1=.5*(XR1OLD+XR1+H*XR1D);
	XR2=.5*(XR2OLD+XR2+H*XR2D);
 	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;
		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.;
		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;
		R1=sqrt((XR1-X)^2+(YR1-Y)^2);
		R2=sqrt((XR2-X)^2+(YR2-Y)^2);
		RES1=R1+R1NOISE+BIAS-R1H;
		RES2=R2+R2NOISE+BIAS-R2H;
		XH=XH+GAIN(1,1)*RES1+GAIN(1,2)*RES2;
		YH=YH+GAIN(2,1)*RES1+GAIN(2,2)*RES2;
		BIASH=BIASH+GAIN(3,1)*RES1+GAIN(3,2)*RES2;
		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);
		TH=atan2(sqrt((R1T*R2T)^2-TEMP^2),TEMP);
		THET=57.3*TH;
		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 0 15000])
figure
plot(ArrayT,ArrayX,ArrayT,ArrayXH),grid
xlabel('Time (Sec)')
ylabel('Receiver Downrange (Ft)')
figure
plot(ArrayT,ArrayY,ArrayT,ArrayYH),grid
xlabel('Time (Sec)')
ylabel('Receiver Altitude (Ft)')
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'