c5l5.m

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

ORDER=2;
TF=20.;
SIGX=.1;
TS=.01;
WN=6.28*.1;
W=6.28*1.;
Z=.7;
A=-Z*WN;
B=WN*sqrt(1.-Z*Z);
A2=.1;
X1=.25;
B2=1.25;
X1D=0.;
B1=1.;
T=0.;
S=0.;
H=.001;
X2DDOLD=0.;
PHI=zeros(ORDER,ORDER);
P=[99999999 0;0 999999999];
IDNP=eye(ORDER);
Q=zeros(ORDER,ORDER);
PHI(1,1)=exp(A*TS)*(-A*sin(B*TS)+B*cos(B*TS))/B;
PHI(1,2)=exp(A*TS)*sin(B*TS)/B;
PHI(2,1)=-WN*WN*exp(A*TS)*sin(B*TS)/B;
PHI(2,2)=exp(A*TS)*(A*sin(B*TS)+B*cos(B*TS))/B;
HMAT=[1 0];
G(1,1)=-(exp(A*TS)*(A*sin(B*TS)-B*cos(B*TS))+B)/(B*(A*A+B*B));
G(2,1)=-exp(A*TS)*sin(B*TS)/B;
PHIT=PHI';
HT=HMAT';
Q(2,2)=0.;
X1H=X1;
X1DH=X1D;
RMAT=SIGX^2;
count=0;
while T<=20
	S=S+H;
	X1OLD=X1;
	X1DOLD=X1D;
	X2=A2*sin(W*T);
	X2D=A2*W*cos(W*T);
	X2DD=-A2*W*W*sin(W*T);
	X1DD=-2.*Z*WN*X1D-WN*WN*X1-X2DD;
	X1=X1+H*X1D;
	X1D=X1D+H*X1DD;
	T=T+H;
	X2=A2*sin(W*T);
	X2D=A2*W*cos(W*T);
	X2DD=-A2*W*W*sin(W*T);
	X1DD=-2.*Z*WN*X1D-WN*WN*X1-X2DD;
	X1=.5*(X1OLD+X1+H*X1D);
	X1D=.5*(X1DOLD+X1D+H*X1DD);
	if S>=(TS-.00001)
		S=0.;
		PHIP=PHI*P;
		PHIPPHIT=PHIP*PHIT;
		M=PHIPPHIT+Q;
		HM=HMAT*M;
		HMHT=HM*HT;
		HMHTR=HMHT+RMAT;
		HMHTRINV=inv(HMHTR);
		MHT=M*HT;
		K=MHT*HMHTRINV;
		KH=K*HMAT;
		IKH=IDNP-KH;
		P=IKH*M;
		XNOISE=SIGX*randn;
		XMEAS=X1+B1+X2+B2+XNOISE;
		XS=XMEAS-X2-B1-B2;
		RES=XS-PHI(1,1)*X1H-PHI(1,2)*X1DH-G(1,1)*X2DDOLD;
		X1HOLD=X1H;
		X1H=PHI(1,1)*X1H+PHI(1,2)*X1DH+G(1,1)*X2DDOLD+K(1,1)*RES;
		X1DH=PHI(2,1)*X1HOLD+PHI(2,2)*X1DH+G(2,1)*X2DDOLD+K(2,1)*RES;
		ERRX1=X1-X1H;
		SP11=sqrt(P(1,1));
		ERRX1D=X1D-X1DH;
		SP22=sqrt(P(2,2));
		X2DDOLD=X2DD;
		SP11P=-SP11;
		SP22P=-SP22;
		count=count+1;
		ArrayT(count)=T;
		ArrayX1(count)=X1;
		ArrayX1H(count)=X1H;
		ArrayX1D(count)=X1D;
		ArrayX1DH(count)=X1DH;
		ArrayERRX1(count)=ERRX1;
		ArraySP11(count)=SP11;
		ArraySP11P(count)=SP11P;
		ArrayERRX1D(count)=ERRX1D;
		ArraySP22(count)=SP22;
		ArraySP22P(count)=SP22P;
	end
end
figure
plot(ArrayT,ArrayX1,ArrayT,ArrayX1H),grid
xlabel('Time (Sec)')
ylabel('X1 AND X1H (Ft)')
axis([0 20 -.2 1])
figure
plot(ArrayT,ArrayX1D,ArrayT,ArrayX1DH),grid
xlabel('Time (Sec)')
ylabel('X1D AND X1DH (Ft)')
axis([0 20 -2 5])
figure
plot(ArrayT,ArrayERRX1,ArrayT,ArraySP11,ArrayT,ArraySP11P),grid
xlabel('Time (Sec)')
ylabel('Error in Estimate of X1 (Ft)')
axis([0 20 -.02 .02])
figure
plot(ArrayT,ArrayERRX1D,ArrayT,ArraySP22,ArrayT,ArraySP22P),grid
xlabel('Time (Sec)')
ylabel('Error in Estimate of X1D (Ft/Sec)')
axis([0 20 -.05 .05])
clc
output=[ArrayT',ArrayX1',ArrayX1H',ArrayX1D',ArrayX1DH'];
save datfil.txt output  -ascii
output=[ArrayT',ArrayERRX1',ArraySP11',ArraySP11P',ArrayERRX1D',ArraySP22',ArraySP22P'];
save covfil.txt output  -ascii
disp 'simulation finished'