基于扩展卡尔曼滤波算法的matlab程序

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;

2、 %统计的初值%滤波算法描述：L=4;alpha=1;kalpha=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)=1

3、2000;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 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'

4、;,0,rangeerror,1,1); xx(i)=Zmeasure(2,i)*cos(Zmeasure(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+1W

5、m(j)=1/(2*(L+ramda);Wc(j)=1/(2*(L+ramda);endWm(1)=ramda/(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=(ran

6、geerror2-Zmeasure(2,i)2*azimutherror2)*sin(Zmeasure(1,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)*s

7、in(Zmeasure(1,i) 0 'end cho=(chol(P*(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+1xpred=xpred+Wm(j)*Xsigmapre(:,j);endNoise1=Anoise; ppred=zeros(Noise1,Nois

8、e1); for j=1:2*L+1ppred=ppred+Wc(j)*(Xsigmapre(:,j)-xpred)*(Xsigmapre(:,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(Xa

9、ugsigma(3,j)/Xaugsigma(1,j) ;Ysigmapre(2,j)=sqrt(Xaugsigma(1,j)2+(Xaugsigma(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

10、+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);% EKF PRO%if i=1ekf_p=xerror xerror/t xyerror xyerror/t;xerror/t 2*xerror/(t2) xyerror/t 2*xyerror/(t2);xyer

11、ror xyerror/t yerror yerror/t;xyerror/t 2*xyerror/(t2) yerror/t 2*yerror/(t2);ekf_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_xpr

12、ed(1)2) 0 ekf_xpred(1)/(ekf_xpred(3)2+ekf_xpred(1)2) 0;ekf_xpred(1)/sqrt(ekf_xpred(3)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'

13、;*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);ukfe

14、rrory(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+(ukfe

15、rrory(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/

16、(i-1);endfigure(1);plot(mseerrorxukf,'r');hold on;plot(mseerrorxekf,'g');hold on;plot(mseerrorx,'.');hold on;ylabel('MSE of X axis','fontsize',15);xlabel('sample number','fontsize',15);legend('UKF','EKF','measurement error&#

17、39;);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