c4l5.m

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

ORDER=3;
BIAS=.00001*32.2;
SF=.000005;
XK=.000001/32.2;
SIGTH=.000001;
G=32.2;
BIASH=0.;
SFH=0.;
XKH=0;
SIGNOISE=.000001;
S=0.;
Q=zeros(ORDER);
PHI=[1 0 0; 0 1 0;0 0 1];
IDNP=eye(ORDER);
PHIT=PHI';
P=[9999999999 0 0;0 99999999999 0;0 0 9999999999999];
count=0;
for THETDEG=0:2:180
	THET=THETDEG/57.3;
	THETNOISE=GAUSSC7(SIGTH);
	THETS=THET+THETNOISE;
	HMAT=[1 G*cos(THETS) (G*cos(THETS))^2];
	HT=HMAT';
	R=[(G*sin(THETS)*SIGTH)^2];
	PHIP=PHI*P;
 	PHIPPHIT=PHIP*PHIT;
 	M=PHIPPHIT+Q;
 	HM=HMAT*M;
 	HMHT=HM*HT;
	HMHTR=HMHT+R;
	HMHTRINV=inv(HMHTR);
	MHT=M*HT;
 	K=MHT*HMHTRINV;
	KH=K*HMAT;
 	IKH=IDNP-KH;
 	P=IKH*M;
 	Z=BIAS+SF*G*cos(THETS)+XK*(G*cos(THETS))^2-G*cos(THET)+G*cos(THETS);
	RES=Z-BIASH-SFH*G*cos(THETS)-XKH*(G*cos(THETS))^2;
	BIASH=BIASH+K(1,1)*RES;
	SFH=SFH+K(2,1)*RES;
	XKH=XKH+K(3,1)*RES;
	SP11=sqrt(P(1,1));
	SP22=sqrt(P(2,2));
	SP33=sqrt(P(3,3));
	SP11P=-SP11;
	SP22P=-SP22;
	SP33P=-SP33;
	BIASERR=BIAS-BIASH;
	SFERR=SF-SFH;
	XKERR=XK-XKH;
	ACTNOISE=G*cos(THETS)-G*cos(THET);
	SIGR=sqrt(R(1,1));
	SIGRP=-SIGR;
	count=count+1;
   	ArrayTHETDEG(count)=THETDEG;
   	ArrayBIAS(count)=BIAS;
   	ArrayBIASH(count)=BIASH;
   	ArraySF(count)=SF;
   	ArraySFH(count)=SFH;
   	ArrayXK(count)=XK;
   	ArrayXKH(count)=XKH;
	ArrayBIASERR(count)=BIASERR;
	ArraySP11(count)=SP11;
	ArraySP11P(count)=SP11P;
	ArraySFERR(count)=SFERR;
	ArraySP22(count)=SP22;
	ArraySP22P(count)=SP22P;
	ArrayXKERR(count)=XKERR;
	ArraySP33(count)=SP33;
	ArraySP33P(count)=SP33P;
	ArrayACTNOISE(count)=ACTNOISE;
	ArraySIGR(count)=SIGR;
	ArraySIGRP(count)=SIGRP;
end
figure
plot(ArrayTHETDEG,ArrayACTNOISE,ArrayTHETDEG,ArraySIGR,ArrayTHETDEG,ArraySIGRP),grid
xlabel('Measurement Angle (Deg)')
ylabel('Actual Measurement Noise (Rad)')
axis([0 180 -.00006 .00006])
figure
plot(ArrayTHETDEG,ArrayBIASERR,ArrayTHETDEG,ArraySP11,ArrayTHETDEG,ArraySP11P),grid
xlabel('Measurement Angle (Deg)')
ylabel('Error in Estimate of Bias (Ft/Sec^2)')
axis([0 180 -.00005 .00005])
figure
plot(ArrayTHETDEG,ArraySFERR,ArrayTHETDEG,ArraySP22,ArrayTHETDEG,ArraySP22P),grid
xlabel('Measurement Angle (Deg)')
ylabel('Error in Estimate of Scale Factor')
axis([0 180 -.00005 .00005])
figure
plot(ArrayTHETDEG,ArraySFERR,ArrayTHETDEG,ArraySP22,ArrayTHETDEG,ArraySP22P),grid
xlabel('Measurement Angle (Deg)')
ylabel('Error in Estimate of G-Sensitive Drift (Sec^2/Ft)')
axis([0 180 -.000005 .000005])
clc
output=[ArrayTHETDEG',ArrayBIAS',ArrayBIASH',ArraySF',ArraySFH',ArrayXK',ArrayXKH'];
save datfil.txt output  -ascii
output=[ArrayTHETDEG',ArrayBIASERR',ArraySP11',ArraySP11P'];
save covfil.txt output  -ascii
disp 'simulation finished'