26TDOA定位的Chan算法MATLAB源代码

欢迎访问GreenSim团队主页→邮箱：[e-mail]第1页TDOA定位的Chan算法MATLAB源代码Chan算法是解决TDOA定位的经典算法，被众多的论文所引用。function[POS_ref,POS1,POS2,POS3,POS4]=TDOA_chan(R,Pbs,Q)%%TDOA定位定位Chan算法%GreenSim团队——专业级算法设计&代写程序%欢迎访问GreenSim团队主页→*********************************************************%CHAN算法,假设移动台与各基站位置较近,需进行三四WLS计算%输入参数:%R(N-1×1):TDOA测量值%Pbs(N×2):基站的坐标,第一列为X,第二列为Y;参考基站坐标位于第一行%输出参数:%POS_ref(2X1):第一次WLS的估计结果,作为参考作最终定位结果的判决%POS1(2X1):定位结果1%POS2(2X1):定位结果2%POS3(2X1):定位结果3%POS4(2X1):定位结果4%这段程序用于自己产生算法输入参数,用于算法测试%delta=10;%TDOA测量误差的标准差,用于产生Q矩阵%M=4;%参考基站外的基站数量%Xb=20;Yb=100;%参考的基站坐标%X=zeros(M,2);%a=2*pi/M;%fori=1:M%生成其他基站的坐标%X(i,1)=400*cos(a*(i-1));%X(i,2)=400*sin(a*(i-1));%end%Xreal=-150;Yreal=200;%移动台真实坐标%%产生TDOA测量结果%Rb=sqrt((Xreal-Xb)^2+(Yreal-Yb)^2);%移动台到基站的真实距离%N=normrnd(0,delta,1,M);%产生TDOA测量误差%Kb=Xb^2+Yb^2;%R=zeros(M,1);%fori=1:M%产生TDOA测量值%R(i)=-Rb+sqrt((Xreal-X(i,1))^2+(Yreal-X(i,2))^2)+N(i);%end%Q=(0.5*eye(M)+0.5*ones(M))*(delta^2);%Pbs=[XbYb;X];%N=5;%[POS_refPOS1POS2POS3POS4]=TDOA_chan(R,Pbs,Q)N=size(Pbs,1);欢迎访问GreenSim团队主页→邮箱：[e-mail]第2页K=zeros(1,N);K=Pbs(:,1).^2+Pbs(:,2).^2;ha=0.5*(R.^2-K(2:N)+K(1))Ga=-[Pbs(2:N,1)-Pbs(1,1)Pbs(2:N,2)-Pbs(1,2)R]%计算第一次WLS估计结果（远距算法）Za=inv(Ga.'*inv(Q)*Ga)*Ga.'*inv(Q)*ha;Ba=diag(Va);Fa=Ba*Q*Ba;Zacov=inv(Ga.'*inv(Fa)*Ga);%第一次ＷＬＳ计算（近距算法）Va2=sqrt((Za2(1)-Pbs(2:N,1)).^2+(Za2(2)-Pbs(2:N,2)).^2);Ba2=diag(Va2);Fa2=Ba2*Q*Ba2;Zacov2=inv(Ga.'*inv(Fa2)*Ga);%第二次ＷＬＳ计算（近距算法）Gb=[10;01;11];Bb2=[Za2(1)-Pbs(1,1)00;0Za2(2)-Pbs(1,2)0;00sqrt((Za2(1)-Pbs(1,1))^2+(Za2(2)-Pbs(1,2))^2)];Fb2=4*Bb2*Zacov2*Bb2;POS_ref=Za2(1:2,1);%第一次WLS的估计结果,作为参考作最终定位结果的判决POS1=sqrt(Zb2)+Pbs(1,:).';%定位结果1POS2=-sqrt(Zb2)+Pbs(1,:).';%定位结果2POS3=[-sqrt(Zb2(1));sqrt(Zb2(2))]+Pbs(1,:).';%定位结果3POS4=[sqrt(Zb2(1));-sqrt(Zb2(2))]+Pbs(1,:).';%定位结果4源代码运行结果展示欢迎访问GreenSim团队主页→邮箱：[e-mail]第3页0.010.10.10.10.20.20.20.20.30.30.30.30.30.40.40.40.40.40.40.50.50.50.50.50.60.60.60.70.70.70.80.80.90.90.950.950.98xdimension(meters)ydimension(meters)TDOABS1(Main)BS2BS3BS401234x1040123456x104