c14l6.m

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

TS=.1;
PHIS=0.;
A0=3.;
A1=.1;
A2=.9;
A0H=0.;
A1H=0.;
A2H=0.;
SIGNOISE=5.;
ORDER=3;
T=0.;
S=0.;
H=.001;
PHI=zeros(ORDER,ORDER);
P=zeros(ORDER,ORDER);
IDNP=eye(ORDER);
Q=zeros(ORDER,ORDER);
RMAT(1,1)=SIGNOISE^2;
PHI(1,1)=1.;
PHI(2,2)=1.;
PHI(3,3)=1.;
P(1,1)=99999999999.;
P(2,2)=99999999999.;
P(3,3)=99999999999.;
count=0;
for T=0:TS:10
	HMAT(1,1)=1.;
	HMAT(1,2)=T;
	HMAT(1,3)=T;
	PHIT=PHI';
	HT=HMAT';
	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;
	K=MHT*HMHTRINV;
	KH=K*HMAT;
	IKH=IDNP-KH;
	P=IKH*M;
	XNOISE=SIGNOISE*randn;
	X=A0+A1*T+A2*T;
	XD=A1+A2;
	XS=X+XNOISE;
	RES=XS-A0H-A1H*T-A2H*T;
	A0H=A0H+K(1,1)*RES;
	A1H=A1H+K(2,1)*RES;
	A2H=A2H+K(3,1)*RES;
	SP11=sqrt(P(1,1));
	SP22=sqrt(P(2,2));
	SP33=sqrt(P(3,3));
	A0HERR=A0-A0H;
	A1HERR=A1-A1H;
	A2HERR=A2-A2H;
	XH=A0H+A1H*T+A2H*T;
	XDH=A1H+A2H;
	SP11P=-SP11;
	SP22P=-SP22;
	SP33P=-SP33;
	count=count+1;
	ArrayT(count)=T;
	ArrayA0(count)=A0;
	ArrayA0H(count)=A0H;
	ArrayA1(count)=A1;
	ArrayA1H(count)=A1H;
	ArrayA2(count)=A2;
	ArrayA2H(count)=A2H;
	ArrayX(count)=X;
	ArrayXH(count)=XH;
	ArrayXD(count)=XD;
	ArrayXDH(count)=XDH;
	ArrayA0HERR(count)=A0HERR;
	ArraySP11(count)=SP11;
	ArraySP11P(count)=SP11P;
	ArrayA1HERR(count)=A1HERR;
	ArraySP22(count)=SP22;
	ArraySP22P(count)=SP22P;
	ArrayA2HERR(count)=A2HERR;
	ArraySP33(count)=SP33;
	ArraySP33P(count)=SP33P;
end
figure
plot(ArrayT,ArrayA1,ArrayT,ArrayA1H),grid
xlabel('Time (Sec)')
ylabel('a1')
axis([0 10 -5 13])
figure
plot(ArrayT,ArrayA2,ArrayT,ArrayA2H),grid
xlabel('Time (Sec)')
ylabel('a2')
axis([0 10 -5 13])
figure
plot(ArrayT,ArrayX,ArrayT,ArrayXH),grid
xlabel('Time (Sec)')
ylabel('x')
axis([0 10 0 12])
figure
plot(ArrayT,ArrayXD,ArrayT,ArrayXDH),grid
xlabel('Time (Sec)')
ylabel('x dot')
axis([0 10 -10 25])
clc
output=[ArrayT',ArrayX',ArrayXH',ArrayXD',ArrayXDH',ArrayA0',ArrayA0H',...
ArrayA1',ArrayA1H',ArrayA2',ArrayA2H'];
save datfil.txt output  -ascii
output=[ArrayT',ArrayA0HERR',ArraySP11',ArraySP11P',ArrayA1HERR',ArraySP22',...
ArraySP22P',ArrayA2HERR',ArraySP33',ArraySP33P'];
save covfil.txt output  -ascii
disp 'simulation finished'