%三邊測量的定位算法
%dA,dB,dC為A,B,C到未知節點(假定坐標[x,y]未知)的模擬測量距離
function [P] = Triangle(A,B,C,dA,dB,dC)
    
    %A,B,C為三個選定的信標節點,節點坐標已知(為便于防真及驗證,代碼中采用的等邊三角形)
    %A = [0,0];
    %B = [25,25*sqrt(3)];
    %C = [50,0];
    
    %定義未知坐標x,y為符號變量
    syms x y;
    
    %距離方程,以信標節點為圓心,信標節點到未知節點的測量距離為半徑作三個圓
    f1 = (A(1)-x)^2+(A(2)-y)^2-dA^2;
    f2 = (B(1)-x)^2+(B(2)-y)^2-dB^2;
    f3 = (C(1)-x)^2+(C(2)-y)^2-dC^2;
    
    %任兩個方程聯立,求任兩圓交點
    s1 = solve(f1,f2); %求A,B兩圓的交點
    s2 = solve(f2,f3); %求B,C兩圓的交點
    s3 = solve(f1,f3); %求A,C兩圓的交點
    
    %將結果(符號變量)轉換為雙精度數值
    x1 = double(s1.x);
    y1 = double(s1.y);
    x2 = double(s2.x);
    y2 = double(s2.y);
    x3 = double(s3.x);
    y3 = double(s3.y);
    
    %選擇內側的三個交點
    %兩圓相交于兩點,距第三個圓心近的為選定交點Pab,Pbc,Pac
    d1(1) = sqrt(((C(1)-x1(1))^2+(C(2)-y1(1))^2));
    d1(2) = sqrt(((C(1)-x1(2))^2+(C(2)-y1(2))^2));
    if d1(1) <= d1(2)
        Pab(1) = x1(1);
        Pab(2) = y1(1);
    else
        Pab(1) = x1(2);
        Pab(2) = y1(2);
    end
    d2(1) = sqrt(((A(1)-x2(1))^2+(A(2)-y2(1))^2));
    d2(2) = sqrt(((A(1)-x2(2))^2+(A(2)-y2(2))^2));
    if d2(1) <= d2(2)
        Pbc(1) = x2(1);
        Pbc(2) = y2(1);
    else
        Pbc(1) = x2(2);
        Pbc(2) = y2(2);
    end
    d3(1) = sqrt(((B(1)-x3(1))^2+(B(2)-y3(1))^2));
    d3(2) = sqrt(((B(1)-x3(2))^2+(B(2)-y3(2))^2));
    if d3(1) <= d3(2)
        Pac(1) = x3(1);
        Pac(2) = y3(1);
    else
        Pac(1) = x3(2);
        Pac(2) = y3(2);
    end
    
    %Pab
    %Pbc
    %Pac
    
    %求三個圓內側三個交點Pab,Pbc,Pac的質心,即為未知節點P,完成定位
    P(1) = (Pab(1)+Pbc(1)+Pac(1))/3;
    P(2) = (Pab(2)+Pbc(2)+Pac(2))/3;