《(完整word版)扩展卡尔曼滤波算法的matlab程序.doc》由会员分享，可在线阅读，更多相关《(完整word版)扩展卡尔曼滤波算法的matlab程序.doc（6页珍藏版）》请在金锄头文库上搜索。
1、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=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);%观测值yy(i)=Zmeasure(2,i)*sin(Zmeasure(1,i); me
4、asureerror=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+ramda);%+1-alpha2+belta
5、; %权值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 xerror/t xyerror xye
6、rror/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)=xestimate+cho(:,j);xgamaP2(:,j)=xe
7、stimate-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);endppred=ppred+processerror;ch
8、or=(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);endypred=zeros(2,1);for j=1:2*L+1 ypr
9、ed=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);xestimate=xpred+K*(Zmeasure(:,i)-ypred);P=pp
10、red-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_xestimate=Zmeasure(2,i)*cos(Zmeasure(1,i) 0 Zmeasu
11、re(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_xpred(1)2) 0 ekf_xpred(3)/sqrt(ekf_xpred(3)2+ekf_x
12、pred(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);
13、 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)+
14、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)=
