global N M    %%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=[0 0;10 0;10 10;0 10];        %%參考（信標）節點分布在四個邊角
undis=L2_distance(actualunknownnodecoor',actualunknownnodecoor');    %%計算未知節點兩兩之間的歐拉距離
refdis=L2_distance(actualunknownnodecoor',refnodecoor');             %%計算未知節點和 信標節點之間的歐拉距離；L2_distance調用函數，計算兩個坐標矩陣所代表的點陣之間歐拉距離
CN=zeros(N);         %%未知節點之間的互聯關系初始化，也即權重函數
CNM=zeros(N,M);      %%未知節點與信標節點之間的互聯關系初始化，也即權重函數
for i=1:N
    for j=1:N
        if(i~=j&&undis(i,j)<=radius)    %%當節點之間距離在通信半徑內，則測距有效，權重函數設為1
            CN(i,j)=1;
        end
    end
end
for i=1:N
    for j=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]);       %%%下面這些是畫圖程序
axis auto;
grid on;
hold on;
plot(actualunknownnodecoor(:,1),actualunknownnodecoor(:,2),'r*');   %%%未知節點實際坐標用紅色*號表示
plot(refnodecoor(:,1),refnodecoor(:,2),'ko');                      %%%參考節點用黑色圓圈表示
plot(calcoor(:,1),calcoor(:,2),'bd');                     %%%定位坐標用鉆石符號表示