基于MDS-MAP无线传感定位算法源程序

1、主程序：globalNM%遑未知节点数目，除已知节点数目N=10;M=4;radius=5;%峨通信半径为5mactualunknownnodecoor=8*rand(N,2)+ones(N,2);%h知节点实际坐标分布在8*8的区域内%actualunknownnodecoor=10*rand(N,2);%refnodecoor=10*rand(M,2);refnodecoor=00;100;1010;010;%考(信标)节点分布在四个边角undis=L2_distance(actualunknownnodecoor',actualunknownnodecoor');%十算未

2、知节点两两之间的欧拉距离refdis=L2_distance(actualunknownnodecoor',refnodecoor');%十算未知节点和信标节点之间的欧拉距离；L2_distance调用函数，计算两个坐标矩阵所代表的点阵之间欧拉距离CN=zeros(N);%h知节点之间的互联关系初始化，也即权重函数CNM=zeros(N,M);%麻知节点与信标节点之间的互联关系初始化，也即权重函数fori=1:N%叫节点之间距离在通信半径内，则测forj=1:Nif(i=j&&undis(i,j)<=radius)距有效，权重函数设为1CN(i,j)=1;

3、endendendfori=1:Nforj=1:Mif(refdis(i,j)<=radius)CNM(i,j)=1;endendenditerative_time=60;absolute_error_value=0.0001;%initial_value=10*randn(N,2);initial_value=zeros(N,2);a0=initial_value;%怵代次数设为60%唯代最小差值%喇始坐标矢I阵设为0a1,segmaX0=matrix_optimal(a0,refnodecoor,undis,refdis,CN,CNM);segmaX1=segmaX0;k=0;whi

4、le(k=0|(segmaX0-segmaX1>absolute_error_value&&k<=iterative_time)k=k+1;segmaX0=segmaX1;a0=a1;a1,segmaX1=matrix_optimal(a0,refnodecoor,undis,refdis,CN,CNM);%巨阵迭代优化，返回下一个坐标a1和当前最小值segmaX1endcalcoor=a1;rectangle('Position',0,0,10,10);%师面这些是画图程序axisauto;gridon;holdon;plot(actualunkn

5、ownnodecoor(:,1),actualunknownnodecoor(:,2),'r*');%殊知节点实际坐标用红色*号表示plot(refnodecoor(:,1),refnodecoor(:,2),'ko');%参考节点用黑色圆圈表示plot(calcoor(:,1),calcoor(:,2),'bd');%1位坐标用钻石符号表示函数1:functiond=L2_distance(a,b,df)%这个程序是下面那个外国佬写的，我借来用一下，计算欧拉距离%L2_DISTANCE-computesEuclideandistancematr

6、ix%E=L2_distance(A,B)%A-(DxM)matrixD维空间M点%B-(DxN)matrixD维空间N个点%df=1,forcediagonalstobezero;0(default),donotforce%Returns:%E-(MxN)EuclideandistancesbetweenvectorsinAandB%Description:%Thisfullyvectorized(VERYFAST!)m-filecomputesthe%Euclideandistancebetweentwovectorsby:%|A-B|=sqrt(|A|A2+|B|A2-2*A.B)%Ex

7、ample:%A=rand(400,100);B=rand(400,200);%d=distance(A,B);%Author:RolandBunschoten%UniversityofAmsterdam%IntelligentAutonomousSystems(IAS)group%Kruislaan4031098SJAmsterdam%tel.(+31)20-5257524%bunschotwins.uva.nl%LastRev:WedOct2008:58:08METDST1999%Tested:PCMatlabv5.2andSolarisMatlabv5.3%Copyrightnotice

8、:Youarefreetomodify,extendanddistribute%thiscodegrantedthattheauthoroftheoriginalcodeis%mentionedastheoriginalauthorofthecode.%FixedbyJBT(3/18/00)toworkfor1-dimensionalvectors%andtowarnforimaginarynumbers.Alsoensuresthat%outputisallreal,andallowstheoptionofforcingdiagonalsto%bezero.if(nargin<2)er

9、ror('Notenoughinputarguments');endif(nargin<3)df=0;%bydefault,donotforce0onthediagonalendif(size(a,1)=size(b,1)error('AandBshouldbeofsamedimensionality');endif(isreal(a)*isreal(b)disp('Warning:runningdistance.mwithimaginarynumbers.Resultsmaybeoff.');endif(size(a,1)=1)a=a;z

10、eros(1,size(a,2);b=b;zeros(1,size(b,2);endaa=sum(a.*a);bb=sum(b.*b);ab=a'*b;d=sqrt(repmat(aa',1size(bb,2)+repmat(bb,size(aa,2)1)-2*ab);%makesureresultisallreald=real(d);%force0onthediagonal?if(df=1)d=d.*(1-eye(size(d);end函数2:functiona1,segma=matrix_optimal(a,b,Da,Dab,Wa,Wab)%矩阵重复优化的核心计算程序，a是

11、N个未知节点坐标矩阵，b是M个信标节点坐标矩阵，Da是未知节点之间白测距矩阵，Dab是未知节点和信标节点之间的测%劭矩阵，亚屣未知节点之间测距的权重函数，Wab是未知节点和信标节点之间测距的权重函数；a1是根据重复优化公式计算出来的下一个迭代的未知节点坐标%矩阵，segma是当前未知节点坐标矩阵a作为定位坐标矩阵和实际的测距的总的平方误差和，也即优化函数的值%此函数综合了matrix_optimala()和matrix_optimalab()两个函数来迭代计算%a(N*K)isthecoordinatesoftheunknownnodes,N:isthenumberofthe%unknownn

12、odes,K:isthedimensional(2or3infact)%b(M*K)isthecoordinatesofthebeaconnodes,M:isthenumberofthe%beaconnodes%Da(N*N)isthemeasureddistancematrixbetweentheunknownnodes%Wa(N*N)istheweightmatrixoftheunknownnodes%Dab(N*M)isthemeasureddistancematrixbetweentheunknownnodeandthe%beaconnode%Wab(N*M)istheweightma

13、trixbetweentheunknownnodeandthebeaconnode%a1(N*K)isthereturnedcoordinatematrixaccordingtotheinputa%segmaistheerrorvalueaccordingtothemeasureddistanceDaandthe%estimatedcoordinates%Author:XieDongfengrow_a=size(a,1);row_b=size(b,1);calculated_disab=L2_distance(a',b');calculated_disa=L2_distance

14、(a',a');%calculated_disabisthedistancebetweentheunknownnodesandbeacon%nodesbycalculatingtheestimatedcoordinates%calculated_disaisthedistancebetweentheunknownnodesby%calculatingV1=zeros(row_a,row_a);V2=zeros(row_a,row_a);V3=zeros(row_a,row_b);V4=zeros(row_b,row_b);fori=1:row_aforj=1:row_aif(i

15、=j)V1(i,j)=-Wa(i,j);V1(i,i)=V1(i,i)+Wa(i,j);endendendfori=1:row_aforj=1:row_bV2(i,i)=V2(i,i)+Wab(i,j);endendV3=Wab;fori=1:row_bforj=1:row_aV4(i,i)=V4(i,i)+Wab(j,i);endendelta=trace(a'*V2*a)-2*trace(a'*V3*b)+trace(b'*V4*b);B1X=zeros(row_a,row_a);B2X=zeros(row_a,row_a);B3X=zeros(row_a,row_

16、b);B4X=zeros(row_b,row_b);lamata=zeros(row_a,row_b);fori=1:row_aforj=1:row_aif(i=j&&calculated_disa(i,j)=0)B1X(i,j)=-Wa(i,j)*Da(i,j)/calculated_disa(i,j);endendendfori=1:row_aforj=1:row_aif(j=i)B1X(i,i)=B1X(i,i)-B1X(i,j);endendendfori=1:row_aforj=1:row_bif(calculated_disab(i,j)=0)lamata(i,j)

17、=Wab(i,j)*Dab(i,j)/calculated_disab(i,j);endendendfori=1:row_aforj=1:row_bB2X(i,i)=B2X(i,i)+lamata(i,j);endendB3X=lamata;fori=1:row_bforj=1:row_aB4X(i,i)=B4X(i,i)+lamata(j,i);endendsita=-2*(trace(a'*B2X*a)-2*trace(a'*B3X*b)+trace(b'*B4X*b);segma1=0;fori=1:row_aforj=1:row_bsegma1=segma1+W

18、ab(i,j)*Dab(i,j)*Dab(i,j);endendsegmaXab=segma1+elta+sita;segma0=0;fori=1:row_aforj=i+1:row_asegma0=segma0+Wa(i,j)*Da(i,j)*Da(i,j);endendsegmaXa=segma0+trace(a'*V1*a)-2*trace(a'*B1X*a);segma=segmaXa+segmaXab;a1=inv(V1+V2)*(B1X+B2X)*a+(V3-B3X)*b);函数3:functiona1,segmaX=matrix_optimala(a,Da,Wa)

19、%矩阵重复优化的核心计算程序之一，a是N个未知节点坐标矩阵，Da是未知节点之间的测距矩阵，Wa未知节点之间测距的权重函数，%a1是根据重复优化公式计算出来的下一个迭代的未知节点坐标%矩阵，segmaX是当前未知节点坐标矩阵a作为定位坐标矩阵和实际的测距的总的平方误差和，也即优化函数的值，此函数只利用未知节点坐标矩阵之间的测距%脸息，计算出来的有可能是相对坐标%a(N*K)isthecoordinatesoftheunknownnodes,N:isthenumberofthe%unknownnodes,K:isthedimensional(2or3infact)%Da(N*N)isthemeas

20、ureddistancematrixbetweentheunknownnodes%Wa(N*N)istheweightmatrixoftheunknownnodes%a1(N*K)isthereturnedcoordinatematrixaccordingtotheinputa%segmaXistheerrorvalueaccordingtothemeasureddistanceDaandthe%estimatedcoordinates%Author:XieDongfengrow_a=size(a,1);%row_a=Ncalculated_disa=L2_distance(a',a&

21、#39;);%calculated_disaisthedistancebetweentheunknownnodesby%calculatingtheestimatedcoordinatesB1X=zeros(row_a,row_a);fori=1:row_aforj=1:row_aif(i=j&&calculated_disa(i,j)=0)B1X(i,j)=-Wa(i,j)*Da(i,j)/calculated_disa(i,j);endendendfori=1:row_aforj=1:row_aif(j=i)B1X(i,i尸B1X(i,i)-B1X(i,j);endende

22、ndV1=zeros(row_a,row_a);fori=1:row_aforj=1:row_aif(i=j)V1(i,j)=-Wa(i,j);V1(i,i)=V1(i,i)+Wa(i,j);endendendV1a=inv(V1+ones(row_a)-1/(row_aA2)*ones(row_a);a1=V1a*B1X*a;segma=0;fori=1:row_aforj=i+1:row_asegma=segma+Wa(i,j)*Da(i,j)*Da(i,j);endendsegmaX=segma+trace(a'*V1*a)-2*trace(a'*B1X*a);函数4:f

23、unctiona1,segmaX=matrix_optimalab(a,b,Dab,Wab)%矩阵重复优化的核心计算程序，a是N个未知节点坐标矩阵，b是M个信标节点坐标矩阵，Dab是未知节点和信标节点之间的测%劭矩阵，Wab是未知节点和信标节点之间测距的权重函数；a1是根据重复优化公式计算出来的下一个迭代的未知节点坐标%矩阵，segmaX是当前未知节点坐标矩阵a作为定位坐标矩阵和实际的测距的总的平方误差和，也即优化函数的值%此函数仅根据未知节点和已知节点之间的测距来进行重复优化计算%a(N*K)isthecoordinatesoftheunknownnodes,N:isthenumberoft

24、he%unknownnodes,K:isthedimensional(2or3infact)%b(M*K)isthecoordinatesofthebeaconnodes,M:isthenumberofthe%beaconnodes%Dab(N*M)isthemeasureddistancematrixbetweentheunknownnodeandthe%beaconnode%Wab(N*M)istheweightmatrixbetweentheunknownnodeandthebeaconnode%a1(N*K)isthereturnedcoordinatematrixaccordingtotheinputa%segmaXistheerrorvalueaccordingtothemeasureddistanceDaandthe%estimatedcoordinates%Author:XieDongfengrow_a=size(a,1);%row_a=Nrow_b=size(b,1);%row_b=Mcalculated_disab=L2_distance(a',b');%calculated_disabisthedistancebetweentheunknownnodesandbeaco