2022年无迹卡尔曼滤波UKF无线传感器网络定位跟踪matlab源码实现 .pdf

%本例对基于量测非线性模型（正切） ，进行了仿真；通过对比分析EKF ，UKF和PF粒子滤波的性能。仿真结果可以看出粒子滤波器比UKF优越， UKF比 EKF性能优越。可作为学习滤波器的参考资料。%存在问题：目前修正效果还不够完美，滤波值在预测值的基础上有所改善，使其接近真实值。clearall;closeall;clc;%Clearcommand window.st=100; %simulation length(time)MC=50;%仿真次数dl=zeros(MC,st+1);de=zeros(MC,st+1);dp=zeros(MC,st+1);%仿真 10 次for time=1:MCdl(time,1)=0;de(time,1)=0;dp(time,1)=0;Q=0.5; %processnoisecovarianceR=320;00.17452;%measurementnoise covariancex0= 0,5,0,7; %initial statex=x0;xA=x(1);%Array:Savethe true X-positionyA=x(3);%Array:SaveY-Positionxobs=x(1); %观测到的坐标yobs=x(3);ZA=;%初始化系统方程系数CV线性模型F=1.0 1.0 0.0 0.0;0.01.0 0.0 0.0;0.00.0 1.0 1.0;0.00.0 0.0 1.0;G=0.50.0;1.00.0;0.00.5;0.01.0;名师资料总结 - - -精品资料欢迎下载 - - - - - - - - - - - - - - - - - - 名师精心整理 - - - - - - - 第 1 页，共 6 页 - - - - - - - - - %事先得到整体过程的实际状态值和观测值for k =1 : st%two equationx=F* x+G* normrnd(0,Q,2,1);%状态方程if x(1)0& x(3)=0z(:,:,k)=sqrt(x(1)2+x(3)2)atan(x(3)/x(1)+normrnd(0,32,1,1)normrnd(0,0.17452,1,1);%观测方程xobs=xobsz(1,1,k)*cos(z(2,1,k);yobs=yobs z(1,1,k)*sin(z(2,1,k);endif x(1)=0z(:,:,k) =sqrt(x(1)2+x(3)2) (atan(x(3)/x(1)+pi) + normrnd(0,32,1,1)normrnd(0,0.17452,1,1);%观测方程xobs=xobsz(1,1,k)*cos(z(2,1,k);yobs=yobs z(1,1,k)*sin(z(2,1,k);endif x(1)0& x(3)0& x(3)0& wxx(3,j)=0wzz(:,:,j)=sqrt(wxx(3,j)2+wxx(1,j)2) atan(wxx(3,j)/wxx(1,j);endif wxx(1,j)=0wzz(:,:,j)=sqrt(wxx(3,j)2+wxx(1,j)2) (atan(wxx(3,j)/wxx(1,j)+pi);endif wxx(1,j)0& wxx(3,j)0& wxx(3,j)=0wzz(:,:,j)=sqrt(wxx(3,j)2+wxx(1,j)2) atan(wxx(3,j)/wxx(1,j);endendzku=wm(1) * wzz(:,:,1);for j =2:2*nnn+1名师资料总结 - - -精品资料欢迎下载 - - - - - - - - - - - - - - - - - - 名师精心整理 - - - - - - - 第 4 页，共 6 页 - - - - - - - - - zku=zku+wm(j) * wzz(:,:,j);endPzu=R;for j =1:2*nnn+1Pzu=Pzu+wc(j)*(wzz(:,:,j) - zku)*(wzz(:,:,j)- zku);endPxzu= wc(1)*(wxx(:,1) - xgepredu)*(wzz(:,:,1)- zku);%这里的 xgeu 是一步预测值for j =2:2*nnn+1Pxzu=Pxzu+wc(j)*(wxx(:,j) - xgepredu)*(wzz(:,:,j) - zku);end%更新Ku=Pxzu*inv(Pzu);xgeu=xgepredu+Ku*( z(:,:,k)- zku);Pu=Pupred - Ku* (Pzu)* Ku;%(Pzu)(-1)ErrorxgeAu= xgeAuxgeu(1);%ygeAu=ygeAuxgeu(3);%de(time,k+1)=sqrt(xgeAu(k+1)-xA(k+1)2+(ygeAu(k+1)-yA(k+1)2);%dp(time,k+1)=sqrt(xPru(k+1)-xA(k+1)2+(yPru(k+1)-yA(k+1)2);end%显示跟踪曲线%k=0:st;%figure;%plot(xA,yA,r*-,xgeAu,ygeAu,g+:,xobs,yobs,b-x);%xA,yA,b*,%xlabel(x);ylabel(y);%legend(True,UKF,Observations);end%统计观测误差曲线与滤波误差曲线dlave=zeros(1,st+1);deave=zeros(1,st+1);dpave=zeros(1,st+1);for i=1:(st+1)dlave(i)=mean(dl(:,i);deave(i)=mean(de(:,i);dpave(i)=mean(dp(:,i);end名师资料总结 - - -精品资料欢迎下载 - - - - - - - - - - - - - - - - - - 名师精心整理 - - - - - - - 第 5 页，共 6 页 - - - - - - - - - figure(1);i=0;plot(i,mean(dlave),b-o,i,mean(deave),g-*); %,i,mean(dpave),r-sfigure(2);i=1:(st+1);plot(i,dlave,b-o,i,deave,g-*); %,i,dpave,r-s名师资料总结 - - -精品资料欢迎下载 - - - - - - - - - - - - - - - - - - 名师精心整理 - - - - - - - 第 6 页，共 6 页 - - - - - - - - -