Trilateration and located the point（x，y，z）

Question

我想找到位於空間某處的未知節點的座標，該空間的參考距離遠離3個或更多節點，它們全都具有已知座標。

此問題與此處所述的Trilateration完全相同Trilateration。

但是，我不明白關於「初步和最終計算」（參考維基百科網站）的部分。我沒有得到P1，P2和P3所在的位置，所以我可以把它們放在這些等式中？

由於

Answer 1

三邊測量是發現的三個球體相交的區域的中心的處理。三個球體中的每一個球體的中心點和半徑必須是已知的。讓我們考慮你的三個示例中心點P1 [-1,1]，P2 [1,1]和P3 [-1，-1]。第一個要求是，P1' 是在原點，所以讓我們調整相應的點通過將偏移向量V [1，-1]到所有三個：

P1' = P1 + V = [0, 0] 
P2' = P2 + V = [2, 0] 
P3' = P3 + V = [0,-2] 

注：調整點由表示'（主要）註釋。

P2'也必須位於x軸上。在這種情況下，它已經做到了，所以不需要調整。

我們將假定每個球的半徑爲2

現在我們有3個方程式（給定的）和3個未知數（X，Y，中心的相交點的Z）。

求解P4'x：

x = (r1^2 - r2^2 + d^2)/2d //(d,0) are coords of P2' 
x = (2^2 - 2^2 + 2^2)/2*2 
x = 1 

求解P4'y：

y = (r1^2 - r3^2 + i^2 + j^2)/2j - (i/j)x //(i,j) are coords of P3' 
y = (2^2 - 2^2 + 0 + -2^2)/2*-2 - 0 
y = -1 

忽略z對2D問題。

P4' = [1，-1]

現在我們轉換回原始通過減去偏移矢量V的座標空間：

P4 = P4' - V = [0,0]

解決方案點P4位於預期的原點。

本文的後半部分描述了一種表示一組點的方法，其中P1不在原點或P2不在x軸上，以使它們適合這些約束。我更願意將它看作翻譯，但兩種方法都會產生相同的解決方案。

編輯：旋轉P2' 到x軸

如果P2' 不位於x軸平移P1到原點之後，我們必須在視圖上執行旋轉。

首先，讓我們來創建一些新的向量作爲例子來使用： P1 = [2,3] P2 = [3,4] P3 = [5,2]

記住，一定要第一將P1轉換爲原點。一如往常，偏移向量V是-P1。在這種情況下，V = [-2，-3]

P1' = P1 + V = [2,3] + [-2,-3] = [0, 0] 
P2' = P2 + V = [3,4] + [-2,-3] = [1, 1] 
P3' = P3 + V = [5,2] + [-2,-3] = [3,-1] 

爲了確定旋轉角度，必須找到並[1,0]（x軸）P2' 之間的角度。

我們可以使用dot product平等：

A dot B = ||A|| ||B|| cos(theta) 

當B是[1,0]，這可以簡化：一個點B是始終只是A的X分量，和|| ||乙（B的大小）總是乘以1，因此可以忽略。

我們現在有斧= ||一|| COS（THETA），我們可以重新排列，以我方最後公式：

theta = acos(Ax/||A||) 

或在我們的情況：

theta = acos(P2'x/||P2'||) 

我們計算P2' 的使用|| A中的大小|| =開方（AX +好哦+ AZ）

||P2'|| = sqrt(1 + 1 + 0) = sqrt(2) 

堵在我們能夠解決THETA

theta = acos(1/sqrt(2)) = 45 degrees 

現在讓我們使用rotation matrix -45度旋轉的場景。 由於P2'y爲正數，並且旋轉矩陣逆時針旋轉，我們將使用負旋轉將P2與x軸對齊（如果P2'y爲負數，則不取消θ）。

R(theta) = [cos(theta) -sin(theta)] 
      [sin(theta) cos(theta)] 

    R(-45) = [cos(-45) -sin(-45)] 
      [sin(-45) cos(-45)] 

我們將使用雙撇號符號''來表示已翻譯和旋轉的矢量。

P1'' = [0,0] (no need to calculate this one) 

P2'' = [1 cos(-45) - 1 sin(-45)] = [sqrt(2)] = [1.414] 
     [1 sin(-45) + 1 cos(-45)] = [0]  = [0] 

P3'' = [3 cos(-45) - (-1) sin(-45)] = [sqrt(2)] = [ 1.414] 
     [3 sin(-45) + (-1) cos(-45)] = [-2*sqrt(2)] = [-2.828] 

現在你可以使用P1''，P2''和P3''來解決P4''。將反向旋轉應用到P4''以獲得P4'，然後反向平移以獲得P4，您的中心點。

要由R撤消旋轉，乘P4 ''（-theta），在這種情況下R（45）。要撤銷翻譯，減去偏移向量V，這與添加P1相同（假設您原先使用-P1作爲V）。

Answer 2

這是我在一個3D打印機固件使用的算法。它避免旋轉座標系，但它可能不是最好的。

有2個解決方案，以三邊測量問題。要獲得第二個，請在二次方程解決方案中將「 - sqrtf」替換爲「+ sqrtf」。

顯然，如果你有足夠的處理器能力和存儲可以使用雙，而不是浮動。

// Primary parameters 
float anchorA[3], anchorB[3], anchorC[3];    // XYZ coordinates of the anchors 

// Derived parameters 
float Da2, Db2, Dc2; 
float Xab, Xbc, Xca; 
float Yab, Ybc, Yca; 
float Zab, Zbc, Zca; 
float P, Q, R, P2, U, A; 

... 

inline float fsquare(float f) { return f * f; } 

... 

// Precompute the derived parameters - they don't change unless the anchor positions change. 
Da2 = fsquare(anchorA[0]) + fsquare(anchorA[1]) + fsquare(anchorA[2]); 
Db2 = fsquare(anchorB[0]) + fsquare(anchorB[1]) + fsquare(anchorB[2]); 
Dc2 = fsquare(anchorC[0]) + fsquare(anchorC[1]) + fsquare(anchorC[2]); 
Xab = anchorA[0] - anchorB[0]; 
Xbc = anchorB[0] - anchorC[0]; 
Xca = anchorC[0] - anchorA[0]; 
Yab = anchorA[1] - anchorB[1]; 
Ybc = anchorB[1] - anchorC[1]; 
Yca = anchorC[1] - anchorA[1]; 
Zab = anchorB[2] - anchorC[2]; 
Zbc = anchorB[2] - anchorC[2]; 
Zca = anchorC[2] - anchorA[2]; 
P = ( anchorB[0] * Yca 
    - anchorA[0] * anchorC[1] 
    + anchorA[1] * anchorC[0] 
    - anchorB[1] * Xca 
    ) * 2; 
P2 = fsquare(P); 
Q = ( anchorB[1] * Zca 
    - anchorA[1] * anchorC[2] 
    + anchorA[2] * anchorC[1] 
    - anchorB[2] * Yca 
    ) * 2; 

R = - ( anchorB[0] * Zca 
     + anchorA[0] * anchorC[2] 
     + anchorA[2] * anchorC[0] 
     - anchorB[2] * Xca 
    ) * 2; 
U = (anchorA[2] * P2) + (anchorA[0] * Q * P) + (anchorA[1] * R * P); 
A = (P2 + fsquare(Q) + fsquare(R)) * 2; 

... 

// Calculate Cartesian coordinates given the distances to the anchors (La, Lb and Lc) 
// First calculate PQRST such that x = (Qz + S)/P, y = (Rz + T)/P. 
// P, Q and R depend only on the anchor positions, so they are pre-computed 
const float S = - Yab * (fsquare(Lc) - Dc2) 
       - Yca * (fsquare(Lb) - Db2) 
       - Ybc * (fsquare(La) - Da2); 
const float T = - Xab * (fsquare(Lc) - Dc2) 
       + Xca * (fsquare(Lb) - Db2) 
       + Xbc * (fsquare(La) - Da2); 

// Calculate quadratic equation coefficients 
const float halfB = (S * Q) - (R * T) - U; 
const float C = fsquare(S) + fsquare(T) + (anchorA[1] * T - anchorA[0] * S) * P * 2 + (Da2 - fsquare(La)) * P2; 

// Solve the quadratic equation for z 
float z = (- halfB - sqrtf(fsquare(halfB) - A * C))/A; 

// Substitute back for X and Y 
float x = (Q * machinePos[2] + S)/P; 
float y = (R * machinePos[2] + T)/P;