c12l2.m

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

SIGNOISE=300.;
BIAS=10000.;
BIASH=0.;
X=0.;
Y=0.;
XR1=1000000.;
YR1=20000.*3280.;
ORDER=1;
TS=1.;
TF=100.;
PHIS=0.;
Q=PHIS*TS;
T=0.;
S=0.;
H=.01;
P=10000.^2;
RMAT=SIGNOISE^2;
count=0;
while T<=TF
	XR1OLD=XR1;
 	XR1D=-14600.;
 	XR1=XR1+H*XR1D;
 	T=T+H;
	XR1D=-14600.;
 	XR1=.5*(XR1OLD+XR1+H*XR1D);
 	S=S+H;
	if S>=(TS-.00001)
		S=0.;
		BIASB=BIASH;
		R1B=sqrt((XR1-X)^2+(YR1-Y)^2)+BIASB;
		R1H=sqrt((XR1-X)^2+(YR1-Y)^2)+BIASH;
		HMAT=1.;
		M=P+Q;
 		HMHT=HMAT*HMAT*M;
 		HMHTR=HMHT+RMAT;
		HMHTRINV=1./HMHTR;
		MHT=M*HMAT;
 		GAIN=MHT*HMHTRINV;
		KH=GAIN*HMAT;
 		IKH=1.-KH;
 		P=IKH*M;
 		R1NOISE=SIGNOISE*randn;
		R1=sqrt((XR1-X)^2+(YR1-Y)^2)+BIAS;
		RES1=R1+R1NOISE-R1H;
		BIASH=BIASB+GAIN*RES1;
		ERRBIAS=BIAS-BIASH;
		SP11=sqrt(P);
		SP11P=-SP11;
		count=count+1;
		ArrayT(count)=T;
		ArrayBIAS(count)=BIAS;
		ArrayBIASH(count)=BIASH;
		ArrayERRBIAS(count)=ERRBIAS;
		ArraySP11(count)=SP11;
		ArraySP11P(count)=SP11P;
		end
end
figure
plot(ArrayT,ArrayBIAS,ArrayT,ArrayBIASH),grid
xlabel('Time (Sec)')
ylabel('Bias (Ft)')
axis([0 100 9800 10100])
figure
plot(ArrayT,ArrayERRBIAS,ArrayT,ArraySP11,ArrayT,ArraySP11P),grid
xlabel('Time (Sec)')
ylabel('Error in Estimate of Bias (Ft)')
axis([0 100 -300 300])
clc
output=[ArrayT',ArrayBIAS',ArrayBIASH',ArrayERRBIAS',ArraySP11',ArraySP11P'];
save datfil.txt output  -ascii
disp 'simulation finished'