c14l8.m

来自「这是zarchan书的fundamentals of kalman filter」· M 代码 · 共 72 行

ORDER =1;
XLOSE=99999999.;
W=.1;
PHIS=10.;
TS=1.;
BIASH=0.;
BIAS=1.;
SIGNOISE=50.;
PHI=zeros(ORDER,ORDER);
P=zeros(ORDER,ORDER);
IDNP=eye(ORDER);
Q=zeros(ORDER,ORDER);
P(1,1)=99999999999999.;
PHI(1,1)=1;
H=zeros(1,ORDER);
R(1,1)=SIGNOISE^2;
PHIT=PHI';
Q(1,1)=PHIS*TS;
count=0;
for T=0:TS:200
	if T>XLOSE 
		R(1,1)=99999999999999.;
	end
	H(1,1)=.5*T*T;
	HT=H';
	PHIP=PHI*P;
	PHIPPHIT=PHIP*PHIT;
	M=PHIPPHIT+Q;
	MHT=M*HT;
	HMHT=H*MHT;
	HMHTR(1,1)=HMHT(1,1)+R(1,1);
	HMHTRINV(1,1)=1./HMHTR(1,1);
	K=MHT*HMHTRINV;
	KH=K*H;
	IKH=IDNP-KH;
	P=IKH*M;
	XNOISE=SIGNOISE*randn;
	X=100.*T-20.*cos(W*T)/W+20./W;
	XD=100.+20.*sin(W*T);
	XDD=20.*W*cos(W*T);
	XGPSS=X+XNOISE;
	XINSS=X+.5*BIAS*T*T;
	XS=XINSS-XGPSS;
	RES=XS-.5*BIASH*T*T;
	BIASH=BIASH+K(1,1)*RES;
	SP11=sqrt(P(1,1));
	BIASERR=BIAS-BIASH;
	XH=XINSS-.5*BIASH*T*T;
	SP11P=-SP11;
	count=count+1;
	ArrayT(count)=T;
	ArrayBIAS(count)=BIAS;
	ArrayBIASH(count)=BIASH;
	ArrayBIASERR(count)=BIASERR;
	ArraySP11(count)=SP11;
	ArraySP11P(count)=SP11P;
	end
figure
plot(ArrayT,ArrayBIAS,ArrayT,ArrayBIASH),grid
xlabel('Time (Sec)')
ylabel('Bias (Ft/Sec^2)')
axis([0 200 0 2])
figure
plot(ArrayT,ArrayBIASERR,ArrayT,ArraySP11,ArrayT,ArraySP11P),grid
xlabel('Time (Sec)')
ylabel('Error in Estimate of Bias (Ft/Sec^2)')
axis([0 200 -.1 .1])
clc
output=[ArrayT',ArrayBIAS',ArrayBIASH',ArrayBIASERR',ArraySP11',ArraySP11P'];
save datfil.txt output  -ascii
disp 'simulation finished'