c2l6.m

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

BIAS=.00001*32.2;
SF=.000005;
XK=.000001/32.2;
SIGTH=0.;
G=32.2;
JJ=0;
count=0;
for THETDEG=0:2:180
	THET=THETDEG/57.3;
	THETNOISE=SIGTH*randn;
	THETS=THET+THETNOISE;
	JJ=JJ+1;
	T(JJ)=32.2*cos(THETS);
	X(JJ)=BIAS+SF*G*cos(THETS)+XK*(G*cos(THETS))^2-G*cos(THET)+G*cos(THETS);
end
N=JJ;
SUM1=0;
SUM2=0;
SUM3=0;
SUM4=0;
SUM5=0;
SUM6=0;
SUM7=0;
for I=1:JJ
	SUM1=SUM1+T(I);
	SUM2=SUM2+T(I)*T(I);
	SUM3=SUM3+X(I);
	SUM4=SUM4+T(I)*X(I);
	SUM5=SUM5+T(I)*T(I)*T(I);
	SUM6=SUM6+T(I)*T(I)*T(I)*T(I);
	SUM7=SUM7+T(I)*T(I)*X(I);
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;
AINV=inv(A);
B(1,1)=SUM3;
B(2,1)=SUM4;
B(3,1)=SUM7;
ANS=AINV*B
for JJ=1:N
     	PZ(JJ)=ANS(1,1)+ANS(2,1)*T(JJ)+ANS(3,1)*T(JJ)*T(JJ);
     	count=count+1;
	ArrayA(count)=T(JJ);
	ArrayB(count)=X(JJ);
	ArrayPZ(count)=PZ(JJ);
end
figure
plot(ArrayA,ArrayB,ArrayA,ArrayPZ),grid
xlabel('gcos(thet) (deg)')
ylabel('Measurement and Estimate')
axis([-35 35 0 .0005])
clc
output=[ArrayA',ArrayB',ArrayPZ'];
save datfil.txt output  -ascii
disp 'simulation finished'