c2l4.m

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

SIGNOISE=1.;
N=0;
TS=.1;
SUM1=0;
SUM2=0;
SUM3=0;
SUM4=0.;
SUMPZ1=0.;
SUMPZ2=0.;
count=0;
for T=0:TS:10
	N=N+1;
	XNOISE=SIGNOISE*randn;
	X1(N)=1;
	XD(N)=0.;
	X(N)=X1(N)+XNOISE;
	SUM1=SUM1+T;
	SUM2=SUM2+T*T;
	SUM3=SUM3+X(N);
	SUM4=SUM4+T*X(N);
	NMAX=N;
end
A(1,1)=N;
A(1,2)=SUM1;
A(2,1)=SUM1;
A(2,2)=SUM2;
B(1,1)=SUM3;
B(2,1)=SUM4;
AINV=inv(A);
ANS=AINV*B;
for I=1:NMAX
	T=.1*(I-1);
	XHAT=ANS(1,1)+ANS(2,1)*T;
	XDHAT=ANS(2,1);
	ERRX=X1(I)-XHAT;
	ERRXD=XD(I)-XDHAT;
	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;
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),grid
xlabel('Time (Sec)')
ylabel('Differences')
clc
output=[ArrayT',ArrayA',ArrayB',ArrayXHAT'];
save datfil.txt output  -ascii
output=[ArrayT',ArrayERRX',ArrayERRXD'];
save covfil.txt output  -ascii
disp 'simulation finished'