c2l5.m

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

SIGNOISE=1.;
TS=.1;
N=0;
SUM1=0.;
SUM2=0.;
SUM3=0.;
SUM4=0.;
SUM5=0.;
SUM6=0.;
SUM7=0.;
SUMPZ1=0.;
SUMPZ2=0.;
count=0;
for T=0:TS:10
	N=N+1;
	XNOISE=SIGNOISE*randn;
	X1(N)=1.;
	XD(N)=0.;
	XDD(N)=0.;
	X(N)=X1(N)+XNOISE;
	SUM1=SUM1+T;
	SUM2=SUM2+T*T;
	SUM3=SUM3+X(N);
	SUM4=SUM4+T*X(N);
	SUM5=SUM5+T^3;
	SUM6=SUM6+T^4;
	SUM7=SUM7+T*T*X(N);
	NMAX=N;
end
A(1,1)=N;
A(1,2)=SUM1;
A(1,3)=SUM2;
A(2,1)=SUM1;
A(2,2)=SUM2;
A(2,3)=SUM5;
A(3,1)=SUM2;
A(3,2)=SUM5;
A(3,3)=SUM6;
B(1,1)=SUM3;
B(2,1)=SUM4;
B(3,1)=SUM7;
AINV=inv(A);
ANS=AINV*B;
for I=1:NMAX
	T=.1*(I-1);
	XHAT=ANS(1,1)+ANS(2,1)*T+ANS(3,1)*T*T;
	XDHAT=ANS(2,1)+2.*ANS(3,1)*T;
	XDDHAT=2.*ANS(3,1);
	ERRX=X1(I)-XHAT;
	ERRXD=XD(I)-XDHAT;
	ERRXDD=XDD(I)-XDDHAT;
	ERRXP=X(I)-XHAT;
	ERRX2=(X1(I)-XHAT)^2;
	ERRXP2=(X(I)-XHAT)^2;
	SUMPZ1=ERRX2+SUMPZ1;
	SUMPZ2=ERRXP2+SUMPZ2;
	count=count+1;
	ArrayT(count)=T;
	ArrayA(count)=X1(I);
	ArrayB(count)=X(I);
	ArrayXHAT(count)=XHAT;
	ArrayERRX(count)=ERRX;
	ArrayERRXD(count)=ERRXD;
	ArrayERRXDD(count)=ERRXDD;
end
figure
plot(ArrayT,ArrayA,ArrayT,ArrayXHAT),grid
xlabel('Time (Sec)')
ylabel('Estimates and Actual')
axis([0 10 0 1.4])
figure
plot(ArrayT,ArrayERRX,ArrayT,ArrayERRXD,ArrayT,ArrayERRXDD),grid
xlabel('Time (Sec)')
ylabel('Differences')
axis([0 10 -.2 .5])
clc
output=[ArrayT',ArrayA',ArrayB',ArrayXHAT'];
save datfil.txt output  -ascii
output=[ArrayT',ArrayERRX',ArrayERRXD',ArrayERRXDD'];
save covfil.txt output  -ascii
disp 'simulation finished'