c2l3.m

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

SIGNOISE=1.;
N=0;
TS=.1;
SUM3=0.;
SUMPZ1=0.;
SUMPZ2=0.;
count=0;
for T=0:TS:10
	N=N+1;
	XNOISE=SIGNOISE*randn;
	X1(N)=1;
	X(N)=X1(N)+XNOISE;
	SUM3=SUM3+X(N);
	NMAX=N;
end
A(1,1)=N;
B(1,1)=SUM3;
AINV(1,1)=1./A(1,1);
ANS(1,1)=AINV(1,1)*B(1,1);
for I=1:NMAX
	T=.1*(I-1);
	XHAT=ANS(1,1);
	ERRX=X1(I)-XHAT;
	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;
	ArrayERRXP(count)=ERRXP;
end
figure
plot(ArrayT,ArrayB,ArrayT,ArrayXHAT),grid
xlabel('T')
ylabel('Estimates and Measurements')
axis([0 10 -2 4])
figure
plot(ArrayT,ArrayA,ArrayT,ArrayXHAT),grid
xlabel('T')
ylabel('Estimates and Signal')
axis([0 10 0 1.4])
figure
plot(ArrayT,ArrayERRX,ArrayT,ArrayERRXP),grid
title('Differences')
xlabel('T')
ylabel('Differences')
axis([0 10 -4 4])
clc
output=[ArrayT',ArrayA',ArrayB',ArrayXHAT'];
save datfil.txt output  -ascii
output=[ArrayT',ArrayERRX',ArrayERRXP'];
save covfil.txt output  -ascii
disp 'simulation finished'