擴(kuò)展卡爾曼濾波算法的matlab程序(共6頁)

1、clear allv=150; %目標(biāo)速度v_sensor=0;%傳感器速度t=1; %掃描周期xradarpositon=0; %傳感器坐標(biāo)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; %統(tǒng)計(jì)的初值L=4;alpha=1;kalpha=0;belta=2;ramda=3-L;azimutherror=0.015; %方位均方誤差ran

2、geerror=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(a)*t;y(i+1)=y(i)+v*sin(a)*t; endfor i=1:200 xradar

3、positon=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);%觀測(cè)值yy(i)=Zmeasure(2,i)*s

4、in(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)=ramda/(L+ramda);Wc(1)=ramda/(L+ram

5、da);%+1-alpha2+belta; %權(quán)值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,i)*cos(Zmeasure(1,i);P=xerror

6、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:L xgamaP1(:,j)=xestim

7、ate+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)*(Xsigmapre(:,j)-xpred)

8、9;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+(Xaugsigma(3,j)2);e

9、ndypred=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(Pzz);

10、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_xe

11、stimate=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

12、)2+ekf_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*(Zmeasur

13、e(:,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

14、-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);mseerror

15、x(i)=sqrt(sumx/(i-1);%噪聲的統(tǒng)計(jì)均方誤差mseerrory(i)=sqrt(sumy/(i-1);mseerrorxukf(i)=sqrt(sumxukf/(i-1);%UKF的統(tǒng)計(jì)均方誤差mseerroryukf(i)=sqrt(sumyukf/(i-1);mseerrorxekf(i)=sqrt(sumxekf/(i-1);%EKF的統(tǒng)計(jì)均方誤差mseerroryekf(i)=sqrt(sumyekf/(i-1);endfigure(1);plot(mseerrorxukf,'r');hold on;plot(mseerrorxekf,'g&#

16、39;);hold on;plot(mseerrorx,'.');hold on;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);