基于MDSMAP无线传感定位算法源程序.docx
《基于MDSMAP无线传感定位算法源程序.docx》由会员分享，可在线阅读，更多相关《基于MDSMAP无线传感定位算法源程序.docx（13页珍藏版）》请在冰豆网上搜索。
基于MDSMAP无线传感定位算法源程序
主程序：
globalNM%%N是未知节点数目，M是已知节点数目
N=10;M=4;
radius=5;%%设通信半径为5m
actualunknownnodecoor=8*rand（N,2）+ones（N,2）;%%未知节点实际坐标分布在8*8的区域内
%%actualunknownnodecoor=10*rand（N,2）;
%%refnodecoor=10*rand（M,2）;
refnodecoor=[00;100;1010;010];%%参考（信标）节点分布在四个边角
undis=L2_distance（actualunknownnodecoor',actualunknownnodecoor'）;%%计算未知节点两两之间的欧拉距离
refdis=L2_distance（actualunknownnodecoor',refnodecoor'）;%%计算未知节点和信标节点之间的欧拉距离；L2_distance调用函数，计算两个坐标矩阵所代表的点阵之间欧拉距离
CN=zeros（N）;%%未知节点之间的互联关系初始化，也即权重函数
CNM=zeros（N,M）;%%未知节点与信标节点之间的互联关系初始化，也即权重函数
fori=1:
N
forj=1:
N
if（i~=j&&undis（i,j）<=radius）%%当节点之间距离在通信半径内，则测距有效，权重函数设为1
CN（i,j）=1;
end
end
end
fori=1:
N
forj=1:
M
if（refdis（i,j）<=radius）
CNM（i,j）=1;
end
end
end
iterative_time=60;%%迭代次数设为60
absolute_error_value=0.0001;%%%迭代最小差值
%%initial_value=10*randn（N,2）;
initial_value=zeros（N,2）;%%%初始坐标矩阵设为0
a0=initial_value;
[a1,segmaX0]=matrix_optimal（a0,refnodecoor,undis,refdis,CN,CNM）;
segmaX1=segmaX0;
k=0;
while（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和当前最小值segmaX1
end
calcoor=a1;
rectangle（'Position',[0,0,10,10]）;%%%下面这些是画图程序
axisauto;
gridon;
holdon;
plot（actualunknownnodecoor（:
1）,actualunknownnodecoor（:
2）,'r*'）;%%%未知节点实际坐标用红色*号表示
plot（refnodecoor（:
1）,refnodecoor（:
2）,'ko'）;%%%参考节点用黑色圆圈表示
plot（calcoor（:
1）,calcoor（:
2）,'bd'）;%%%定位坐标用钻石符号表示
函数1：
functiond=L2_distance（a,b,df）
%%%%这个程序是下面那个外国佬写的，我借来用一下，计算欧拉距离
%L2_DISTANCE-computesEuclideandistancematrix
%
%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||^2+||B||^2-2*A.B）
%
%Example:
%A=rand（400,100）;B=rand（400,200）;
%d=distance（A,B）;
%Author:
RolandBunschoten
%UniversityofAmsterdam
%IntelligentAutonomousSystems（IAS）group
%Kruislaan4031098SJAmsterdam
%tel.（+31）20-5257524
%bunschot@wins.uva.nl
%LastRev:
WedOct2008:
58:
08METDST1999
%Tested:
PCMatlabv5.2andSolarisMatlabv5.3
%Copyrightnotice:
Youarefreetomodify,extendanddistribute
%thiscodegrantedthattheauthoroftheoriginalcodeis
%mentionedastheoriginalauthorofthecode.
%FixedbyJBT（3/18/00）toworkfor1-dimensionalvectors
%andtowarnforimaginarynumbers.Alsoensuresthat
%outputisallreal,andallowstheoptionofforcingdiagonalsto
%bezero.
if（nargin<2）
error（'Notenoughinputarguments'）;
end
if（nargin<3）
df=0;%bydefault,donotforce0onthediagonal
end
if（size（a,1）~=size（b,1））
error（'AandBshouldbeofsamedimensionality'）;
end
if~（isreal（a）*isreal（b））
disp（'Warning:
runningdistance.mwithimaginarynumbers.Resultsmaybeoff.'）;
end
if（size（a,1）==1）
a=[a;zeros（1,size（a,2））];
b=[b;zeros（1,size（b,2））];
end
aa=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）;
%makesureresultisallreal
d=real（d）;
%force0onthediagonal?
if（df==1）
d=d.*（1-eye（size（d）））;
end
函数2：
function[a1,segma]=matrix_optimal（a,b,Da,Dab,Wa,Wab）
%%%矩阵重复优化的核心计算程序，a是N个未知节点坐标矩阵，b是M个信标节点坐标矩阵，Da是未知节点之间的测距矩阵，Dab是未知节点和信标节点之间的测
%%%距矩阵，Wa是未知节点之间测距的权重函数，Wab是未知节点和信标节点之间测距的权重函数；a1是根据重复优化公式计算出来的下一个迭代的未知节点坐标
%%%矩阵，segma是当前未知节点坐标矩阵a作为定位坐标矩阵和实际的测距的总的平方误差和，也即优化函数的值
%%%此函数综合了matrix_optimala（）和matrix_optimalab（）两个函数来迭代计算
%%%a（N*K）isthecoordinatesoftheunknownnodes,N:
isthenumberofthe
%%%unknownnodes,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）istheweightmatrixbetweentheunknownnodeandthebeaconnode
%%%a1（N*K）isthereturnedcoordinatematrixaccordingtotheinputa
%%%segmaistheerrorvalueaccordingtothemeasureddistanceDaandthe
%%%estimatedcoordinates
%%%Author:
XieDongfeng
row_a=size（a,1）;
row_b=size（b,1）;
calculated_disab=L2_distance（a',b'）;
calculated_disa=L2_distance（a',a'）;
%%%calculated_disabisthedistancebetweentheunknownnodesandbeacon
%%%nodesbycalculatingtheestimatedcoordinates
%%%calculated_disaisthedistancebetweentheunknownnodesby
%%%calculating
V1=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_a
升级会员 