资源描述：
《无迹卡尔曼滤波UKF无线传感器网络定位跟踪matlab源码实现.doc》由会员上传分享，免费在线阅读，更多相关内容在教育资源-天天文库。
1、%本例对基于量测非线性模型（正切），进行了仿真；通过对比分析EKF，UKF和PF粒子滤波的性能。仿真结果可以看出粒子滤波器比
UKF优越，UKF比EKF性能优越。可作为学习滤波器的参考资料。%存在问题：目前修正效果还不够完美，滤波值在预测值的基础上有所改善，使其接近真实值。clearall;closeall;clc;%Clearcommandwindow.st=100;%simulationlength(time)MC=50;%仿真次数dl=zeros(MC,st+1);de=zeros(MC,st+1);
2、dp=zeros(MC,st+1);%仿真10次fortime=1:MCdl(time,1)=0;de(time,1)=0;dp(time,1)=0;Q=0.5;%processnoisecovarianceR=[3^20;00.1745^2];%measurementnoisecovariancex0=[0,5,0,7]';%initialstatex=x0;xA=[x(1)];%Array:SavethetrueX-positionyA=[x(3)];%Array:SaveY-Positionxobs=
3、[x(1)];%观测到的坐标yobs=[x(3)];ZA=[];%初始化系统方程系数CV线性模型F=[1.01.00.00.0;0.01.00.00.0;0.00.01.01.0;0.00.00.01.0];G=[0.50.0;1.00.0;0.00.5;0.01.0];%事先得到整体过程的实际状态值和观测值fork=1:st%twoequationx=F*x+G*normrnd(0,Q,2,1);%状态方程ifx(1)>0&&x(3)>=0z(:,:,k)=[sqrt(x(1)^2+x(3)^2)atan
4、(x(3)/x(1))]'+[normrnd(0,3^2,1,1)normrnd(0,0.1745^2,1,1)]';%观测
方程xobs=[xobsz(1,1,k)*cos(z(2,1,k))];yobs=[yobsz(1,1,k)*sin(z(2,1,k))];endifx(1)<0&&x(3)>=0z(:,:,k)=[sqrt(x(1)^2+x(3)^2)(atan(x(3)/x(1))+pi)]'+[normrnd(0,3^2,1,1)normrnd(0,0.1745^2,1,1)]';
%观测方程
5、xobs=[xobsz(1,1,k)*cos(z(2,1,k))];yobs=[yobsz(1,1,k)*sin(z(2,1,k))];endifx(1)<0&&x(3)<=0z(:,:,k)=[sqrt(x(1)^2+x(3)^2)(atan(x(3)/x(1))-pi)]'+[normrnd(0,3^2,1,1)normrnd(0,0.1745^2,1,1)]';
%观测方程xobs=[xobsz(1,1,k)*cos(z(2,1,k))];yobs=[yobsz(1,1,k)*sin(z(2,1,k)
6、)];endifx(1)>0&&x(3)<=0z(:,:,k)=[sqrt(x(1)^2+x(3)^2)atan(x(3)/x(1))]'+[normrnd(0,3^2,1,1)normrnd(0,0.1745^2,1,1)]';%观测
方程xobs=[xobsz(1,1,k)*cos(z(2,1,k))];yobs=[yobsz(1,1,k)*sin(z(2,1,k))];endxA=[xAx(1)];yA=[yAx(3)];dl(time,k+1)=sqrt((xobs(k+1)-x(1))^2+(yo
7、bs(k+1)-x(3))^2);end%k=0:st;%figure(1);%plot(xA,yA,'b*',0,0,'ro');%xlabel('x');ylabel('y');%legend('TargetPosition','ObservationPosition');%UKFPu=[2,0,0,0;0,0.5,0,0;0,0,3,0;0,0,0,0.5].^2;%协方差矩阵初始化xgeu=x0;xgeAu=[xgeu(1)];%后面代表的是滤波后的估计位置ygeAu=[xgeu(3)];xPru=
8、[xgeu(1)];%代表每一步目标的预测位置yPru=[xgeu(3)];alpha=0.01;%0.5beta=2;nnn=4;kappa=-1;lamda=alpha^2*(nnn+kappa)-nnn;%一般的方法：W0=v/(v+n),Wi=0.5/(v+n),i=1,...,2n;一般(v+n)==3%这儿n=4,v=-1;W0=-1/3,Wi=1/6wm=lamda/(nnn+lamda);wc=w
