《扩展卡尔曼滤波算法的matlab程序.doc》由会员分享，可在线阅读，更多相关《扩展卡尔曼滤波算法的matlab程序.doc（7页珍藏版）》请在taowenge.com淘文阁网|工程机械CAD图纸|机械工程制图|CAD装配图下载|SolidWorks_CaTia_CAD_UG_PROE_设计图分享下载上搜索。
1、【精品文档】如有侵权，请联系网站删除，仅供学习与交流扩展卡尔曼滤波算法的matlab程序.精品文档.clear allv=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=
2、0;belta=2;ramda=3-L;azimutherror=0.015; %方位均方误差rangeerror=100; %距离均方误差processnoise=1; %过程噪声均方差tao=t3/3 t2/2 0 0;t2/2 t 0 0;0 0 t3/3 t2/2;0 0 t2/2 t; % the input matrix of process G=t2/2 0 t 0 0 t2/20 t ;a=35*pi/180;a_v=5/100;a_sensor=45*pi/180;x(1)=8000; %初始位置y(1)=12000;for i=1:200x(i+1)=x(i)+v*cos(
3、a)*t;y(i+1)=y(i)+v*sin(a)*t; endfor i=1:200 xradarpositon=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(
4、1,i);%观测值yy(i)=Zmeasure(2,i)*sin(Zmeasure(1,i); measureerror=azimutherror2 0;0 rangeerror2;processerror=tao*processnoise;vNoise = size(processerror,1);wNoise = size(measureerror,1);A=1 t 0 0;0 1 0 0;0 0 1 t;0 0 0 1;Anoise=size(A,1);for j=1:2*L+1Wm(j)=1/(2*(L+ramda);Wc(j)=1/(2*(L+ramda);endWm(1)=ramd
5、a/(L+ramda);Wc(1)=ramda/(L+ramda);%+1-alpha2+belta; %权值if i=1 xerror=rangeerror2*cos(Zmeasure(1,i)2+Zmeasure(2,i)2*azimutherror2*sin(Zmeasure(1,i)2;yerror=rangeerror2*sin(Zmeasure(1,i)2+Zmeasure(2,i)2*azimutherror2*cos(Zmeasure(1,i)2;xyerror=(rangeerror2-Zmeasure(2,i)2*azimutherror2)*sin(Zmeasure(1,
6、i)*cos(Zmeasure(1,i);P=xerror xerror/t xyerror xyerror/t;xerror/t 2*xerror/(t2) xyerror/t 2*xyerror/(t2);xyerror xyerror/t yerror yerror/t;xyerror/t 2*xyerror/(t2) yerror/t 2*yerror/(t2);xestimate=Zmeasure(2,i)*cos(Zmeasure(1,i) 0 Zmeasure(2,i)*sin(Zmeasure(1,i) 0 ;end cho=(chol(P*(L+ramda);%for j=1
7、: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+1xpred=xpred+Wm(j)*Xsigmapre(:,j);endNoise1=Anoise; ppred=zeros(Noise1,Noise1); for j=1:2*L+1ppred=ppred+Wc(j)*(Xsigmapre(:,j)-xpred)*(Xs
8、igmapre(:,j)-xpred);endppred=ppred+processerror;chor=(chol(L+ramda)*ppred);for j=1:LXaugsigmaP1(:,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(2,j)=sqrt(Xaugsigma(1,j)2+(Xaug
9、sigma(3,j)2);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)*(Ysigmapre(:,j)-ypred);endK=Pxy*inv
10、(Pzz);xestimate=xpred+K*(Zmeasure(:,i)-ypred);P=ppred-K*Pzz*K; xukf(i)=xestimate(1,1); yukf(i)=xestimate(3,1);% EKF PRO%if i=1ekf_p=xerror xerror/t xyerror xyerror/t;xerror/t 2*xerror/(t2) xyerror/t 2*xyerror/(t2);xyerror xyerror/t yerror yerror/t;xyerror/t 2*xyerror/(t2) yerror/t 2*yerror/(t2);ekf_
11、xestimate=Zmeasure(2,i)*cos(Zmeasure(1,i) 0 Zmeasure(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) 0 ekf_xpred(1)/(ekf_xpred(3)2+ekf_xpred(1)2) 0;ekf_xpred(1)/sqrt(ekf_xpred(3)2+ekf_
12、xpred(1)2) 0 ekf_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); tr
13、aceekf(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
14、)=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
15、-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);hold on;plot(mseerrorxekf,g);hold on;plot(mseerrorx,.);hold o
16、n;ylabel(MSE of X axis,fontsize,15);xlabel(sample number,fontsize,15);legend(UKF,EKF,measurement error);figure(2)plot(mseerroryukf,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);