扩展卡尔曼滤波算法的matlab程序.doc

clearallv=150;%%目标速度v_sensor=0;%%传感器速度t=1;%%扫描周期xradarpositon=0;%%传感器坐标yradarpositon=0;%%ppred=zeros(4,4);Pzz=zeros(2,2);Pxx=zeros(4,2);xpred=zeros(4,1);ypred=zeros(2,1);sumx=0;sumy=0;sumxukf=0;sumyukf=0;sumxekf=0;sumyekf=0;%%%统计的初值L=4;alpha=1;kalpha=0;belta=2;ramda=3-L;azimutherror=0.015;%%方位均方误差rangeerror=100;%%距离均方误差processnoise=1;%%过程噪声均方差tao=[t^3/3t^2/200;t^2/2t00;00t^3/3t^2/2;00t^2/2t];%%theinputmatrixofprocessG=[t^2/20t00t^2/20t];a=35*pi/180;a_v=5/100;a_sensor=45*pi/180;x(1)=8000;%%初始位置y(1)=12000;fori=1:200x(i+1)=x(i)+v*cos(a)*t;y(i+1)=y(i)+v*sin(a)*t;endfori=1:200xradarpositon=0;yradarpositon=0;Zmeasure(1,i)=atan((y(i)-yradarpositon)/(x(i)-xradarpositon))+random('Normal',0,azimutherror,1,1);Zmeasure(2,i)=sqrt((y(i)-yradarpositon)^2+(x(i)-xradarpositon)^2)+random('Normal',0,rangeerror,1,1);xx(i)=Zmeasure(2,i)*cos(Zmeasure(1,i));%%观测值yy(i)=Zmeasure(2,i)*sin(Zmeasure(1,i));measureerror=[azimutherror^20;0rangeerror^2];processerror=tao*processnoise;vNoise=size(processerror,1);wNoise=size(measureerror,1);A=[1t00;0100;001t;0001];Anoise=size(A,1);forj=1:2*L+1Wm(j)=1/(2*(L+ramda));Wc(j)=1/(2*(L+ramda));endWm(1)=ramda/(L+ramda);Wc(1)=ramda/(L+ramda);%+1-alpha^2+belta;%%%权值ifi==1xerror=rangeerror^2*cos(Zmeasure(1,i))^2+Zmeasure(2,i)^2*azimutherror^2*sin(Zmeasure(1,i))^2;yerror=rangeerror^2*sin(Zmeasure(1,i))^2+Zmeasure(2,i)^2*azimutherror^2*cos(Zmeasure(1,i))^2;xyerror=(rangeerror^2-Zmeasure(2,i)^2*azimutherror^2)*sin(Zmeasure(1,i))*cos(Zmeasure(1,i));P=[xerrorxerror/txyerrorxyerror/t;xerror/t2*xerror/(t^2)xyerror/t2*xyerror/(t^2);xyerrorxyerror/tyerroryerror/t;xyerror/t2*xyerror/(t^2)yerror/t2*yerror/(t^2)];xestimate=[Zmeasure(2,i)*cos(Zmeasure(1,i))0Zmeasure(2,i)*sin(Zmeasure(1,i))0]';endcho=(chol(P*(L+ramda)))';%forj=1:LxgamaP1(:,j)=xestimate+cho(:,j);xgamaP2(:,j)=xestimate-cho(:,j);endXsigma=[xestimatexgamaP1xgamaP2];F=A;Xsigmapre=F*Xsigma;xpred=zeros(Anoise,1);forj=1:2*L+1xpred=xpred+Wm(j)*Xsigmapre(:,j);endNoise1=Anoise;ppred=zeros(Noise1,Noise1);forj=1:2*L+1ppre