擴展卡爾曼濾波算法的matlab程序3

1、clear allfor i=1:200v=150; % 目標(biāo)速度 v_sensor=0;% 傳感器速度 t=1; % 掃描周期x(i+1)=x(i)+v*cos(a)*t; y(i+1)=y(i)+v*sin(a)*t; endxradarpositon=0; % 傳感器坐標(biāo)yradarpositon=0; %for i=1:200 xradarpositon=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;yradar

2、positon=0;zmeasure(1,i)=atan(y(i)-yradarpositon)/(x(i)-xradarposit on)+random('normal',0,azimutherror,1,1);zmeasure(2,i)=sqrt(y(i)-yradarpositon)a2+(x(i)-xradarpositon)a2)+random('normal',0,rangeerror,1,1);xx(i)=zmeasure(2,i)*cos(zmeasure(1,i);%觀測值sumyukf=0;sumxekf=0;sumyekf=0; % 統(tǒng)計的

3、初值yy(i)=zmeasure(2,i)*sin(zmeasure(1,i);measureerror=azimutherrora2 0;0 rangeerrora2;processerror=tao*processnoise;l=4;vnoise = size(processerror ,1);alpha=1; kalpha=0; belta=2;wnoise = size(measureerror ,1);a=1 t 0 0;ramda=3-l;0 1 0 0;0 0 1 t;azimutherror=0.015; % 方位均方誤差 rangeerror=100; % 距離均方誤差 pr

4、ocessnoise=1; % 過程噪聲均方差0 0 0 1;anoise=size(a,1);for j=1:2*l+1tao=ta3/3 22/2 0 0;wm(j)=1/(2*(l+ramda);22/2 to 0;0 0 ta3/3 ta2/2;0 0 ta2/2 t; % the input matrix of processwc(j)=1/(2*(l+ramda);endg=ta2/2 0t 0wm(1)=ramda/(l+ramda);wc(1)=ramda/(l+ramda);%+1-alphaa2+belta;% 權(quán)0 ta2/2值0 t ;if i=1a=35*pi/180

5、; a_v=5/100;xerror=rangeerrora2*cos(zmeasure(1,i)a2+zmeasure(2 ,i)a2*azimutherrora2*sin(zmeasure(1,i)a2;yerror=rangeerrora2*sin(zmeasure(1,i)a2+zmeasure(2,a_sensor=45*pi/180;x(1)=8000; % 初始位置i)a2*azimutherrora2*cos(zmeasure(1,i)a2;xyerror=(rangeerrora2-zmeasure(2,i)a2*azimutherrora2y(1)=12000;)*sin(

6、zmeasure(1,i)*cos(zmeasure(1,i);p=xerror xerror/t xyerror xyerror/t;xerror/t 2*xerror/(ta2) xyerror/t 2*xyerror/(ta2);xyerror xyerror/t yerror yerror/t; xyerror/t 2*xyerror/(ta2) yerror/t 2*yerror/(ta2);xestimate=zmeasure(2,i)*cos(zmeasure(1,i)0zmeasure(2,i)*sin(zmeasure(1,i) 0 'endcho=(chol(p*(

7、l+ramda)'%for j=1:l xgamap1(:,j)=xestimate+cho(:,j); xgamap2(:,j)=xestimate-cho(:,j);endxsigma=xestimate xgamap1 xgamap2;f=a;xsigmapre=f*xsigma;xpred=zeros(anoise,1);for j=1:2*l+1 xpred=xpred+wm(j)*xsigmapre(:,j);endnoise1=anoise; ppred=zeros(noise1,noise1);for j=1:2*l+1 ppred=ppred+wc(j)*(xsigm

8、apre(:,j)-xpred)*(xsigmapre( :,j)-xpred)'endppred=ppred+processerror;chor=(chol(l+ramda)*ppred)'for j=1:l xaugsigmap1(:,j)=xpred+chor(:,j); xaugsigmap2(:,j)=xpred-chor(:,j);endxaugsigma=xpred xaugsigmap1 xaugsigmap2 ;for j=1:2*l+1ysigmapre(1,j)=atan(xaugsigma(3,j)/xaugsigma(1,j) ; ysigmapre(

9、2,j)=sqrt(xaugsigma(1,j)a2+(xaugsigma(3,j)a2);endypred=zeros(2,1);for j=1:2*l+1 ypred=ypred+wm(j)*ysigmapre(:,j);endpzz=zeros(2,2);for j=1:2*l+1pzz=pzz+wc(j)*(ysigmapre(:,j)-ypred)*(ysigmapre(:,j)-ypred)'endpzz=pzz+measureerror;pxy=zeros(anoise,2);for j=1:2*l+1pxy=pxy+wc(j)*(xaugsigma(:,j)-xpred

10、)*(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%if i=1ekf_p=xerror xerror/t xyerror xyerror/t;xerror/t 2*xerror/(ta2) xyerror/t 2*xyerror/(ta2);xyerror xyerror/t yerror yerror/t;xyerro

11、r/t 2*xyerror/(ta2) yerror/t 2*yerror/(ta2);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)a2+ekf_xpred(1)a2)0ekf_xpred(1)/(ekf_xpred(3

12、)a2+ekf_xpred(1)a2) 0;ekf_xpred(1)/sqrt(ekf_xpred(3)a2+ekf_xpred(1)a2)0ekf_xpred(3)/sqrt(ekf_xpred(3)a2+ekf_xpred(1)a2) 0;ekf_z(1,1)=atan(ekf_xpred(3)/ekf_xpred(1) ;ekf_z(2,1)=sqrt(ekf_xpred(1)a2+(ekf_xpred(3)a2);phhp=h*ekf_ppred*h'+measureerror; ekf_k=ekf_ppred*h'*inv(phhp);ekf_p=(eye(l)-ek

13、f_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)+yradarpos

14、iton-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(if2);sumy=sumy+(errory(i)a2); sumxukf=sumxukf+(ukferrorx(if2); sumyukf=sumyukf+(ukferrory(i)a2);sumxekf=sumxekf+

15、(ekferrorx(i)a2);sumyekf=sumyekf+(ekferrory(i)a2);mseerrorx(i)=sqrt(sumx/(i-1);% 噪聲的統(tǒng)計均方誤差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);的統(tǒng)計均方誤的統(tǒng)計均方誤endfigure(2) plot(m

16、seerroryukf,'r');hold on;plot(mseerroryekf,'g');hold on;plot(mseerrory,'.');hold on;ylabel('mse of y axis','fontsize',15); xlabel('sample number','fontsize',15); legend('ukf','ekf','measurement error');figure(3) plot(x,y);hold on;plot(xekf,yekf,'g');hold on;plot(xukf,yukf,'r');hold on;plot(xx,yy,'m');ylabel(' x ','fontsize',15); xlabel('y','fontsize',15); legend('true','ukf','ekf','measurements');figure(1);plot(mseerrorxukf,&#