搜索
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+1ppred=ppred+Wc(j)*(Xsigmapre(:,j)-xpred)*(Xsigmapre(:,j)-xpred)';endppred=ppred+processerror;chor=(chol((L+ramda)*ppred))';forj=1:LXaugsigmaP1(:,j)=xpred+chor(:,j);XaugsigmaP2(:,j)=xpred-chor(:,j);endXaugsigma=[xpredXaugsigmaP1XaugsigmaP2];forj=1:2*L+1Ysigmapre(1,j)=atan(Xaugsigma(3,j)/Xaugsigma(1,j));Ysigmapre(2,j)=sqrt((Xaugsigma(1,j))^2+(Xaugsigma(3,j))^2);endypred=zeros(2,1);forj=1:2*L+1ypred=ypred+Wm(j)*Ysigmapre(:,j);endPzz=zeros(2,2);forj=1:2*L+1Pzz=Pzz+Wc(j)*(Ysigmapre(:,j)-ypred)*(Ysigmapre(:,j)-ypred)';endPzz=Pzz+measureerror;Pxy=zeros(Anoise,2);forj=1:2*L+1Pxy=Pxy+Wc(j)*(Xaugsigma(:,j)-xpred)*(Ysigmapre(:,j)-ypred)';endK=Pxy*inv(Pzz);xestimate=xpred+K*(Zmeasure(:,i)-ypred);P=ppred-K*Pzz*K';xukf(i)=xestimate(1,1);yukf(i)=xestimate(3,1);%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%EKFPRO%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%ifi==1ekf_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)];ekf_xestimate=[Zmeasure(2,i)*cos(Zmeasure(1,i))0Zmeasure(2,i)*sin(Zmeasure(1,i))0]';ekf_xpred=ekf_xestimate;end;F=A;ekf_xpred=F*ekf_xestimate;ekf_ppred=F*ekf_p*F'+processerror;H=[-ekf_xpred(3)/(ekf_xpred(3)^2+ekf_xpred(1)^2)0ekf_xpred(1)/(ekf_xpred(3)^2+ekf_xpred(1)^2)0;ekf_xpred(1)/sqrt(ekf_xpred(3)^2+ekf_xpred(1)^2)0ekf_xpred(3)/sqrt(ekf_xpred(3)^2+ekf_xpred(1)^2)0];ekf_z(1,1)=atan(ekf_xpred(3)/ekf_xpred(1));ekf_z(2,1)=sqrt((ekf_xpred(1))^2+(ekf_xpred(3))^2);PHHP=H*ekf_ppred*H'+measureerror;ekf_K=ekf_ppred*H'*inv(PHHP);ekf_p=(eye(L)-ekf_K*H)*ekf_ppred;ekf_xestimate=ekf_xpred+ekf_K*(Zmeasure(:,i)-ekf_z);traceekf(i)=trace(ekf_p);xekf(i)=ekf_xestimate(1,1);yekf(i)=ekf_xestimate(3,1);errorx(i)=xx(i)+xradarpositon-x(i);errory(i)=yy(i)+yradarpositon-y(i);ukferrorx(i)=xestimate(1)+xradarpositon-x(i);ukferrory(i)=xestimate(3)+yradarpositon-y(i);ekferrorx(i)=ekf_xestimate(1)+xradarpositon-x(i);ekferrory(i)=ekf_xestimate(3)+yradarpositon-y(i);aa(i)=xx(i)+xradarpositon-x(i);;bb(i)=yy(i)+yradarpositon-y(i);sumx=sumx+(errorx(i)^2);sumy=sumy+(errory(i)^2);sumxukf=sumxukf+(ukferrorx(i)^2);sumyukf=sumyukf+(ukferrory(i)^2);sumxekf=sumxekf+(ekferrorx(i)^2);sumyekf=sumyekf+(ekferrory(i)^2);mseerrorx(i)=sqrt(sumx/(i-1));%噪声的统计均方误差mseerrory(i)=sqrt(sumy/(i-1));mseerrorxukf(i)=sqrt(sumxukf/(i-1));%UKF的统计均方误差mseerroryukf(i)=sqrt(sumyukf/(i-1));mseerrorxekf(i)=sqrt(sumxekf/(i-1));%EKF的统计均方误差mseerroryekf(i)=sqrt(sumyekf/(i-1));endfigure(1);plot(mseerrorxukf,'r');holdon;plot(mseerrorxekf,'g');holdon;plot(mseerrorx,'.');holdon;ylabel('MSEofXaxis','fontsize',15);xlabel('samplenumber','fontsize',15);legend('UKF','EKF','measurementerror');figure(2)plot(mseerroryukf,'r');holdon;plot(mseerroryekf,'g');holdon;plot(mseerrory,'.');holdon;ylabel('MSEofYaxis','fontsize',15);xlabel('samplenumber','fontsize',15);legend('UKF','EKF','measurementerror');figure(3)plot(x,y);holdon;plot(xekf,yekf,'g');holdon;plot(xukf,yukf,'r');holdon;plot(xx,yy,'m