三角测量和定位点（x，y，z）
我想找到一个未知节点的坐标，这个未知节点位于空间的某个位置，该节点的参考距离远离3个或更多的节点，它们都有已知的坐标。
此问题与Trilateration完全相同，如此处所述的Trilateration。
但是，我不明白关于“初步和最终计算”的部分（请参阅维基百科网站）。 我没有得到P1，P2和P3所在的位置，所以我可以把它们放在这些等式中？
谢谢
三边测量是找出三个球体相交区域的中心的过程。 三个球体中的每一个球体的中心点和半径必须是已知的。
让我们考虑你的三个示例中心点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]
注意：调整后的点由'（prime）注释表示。
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
针对2D问题忽略z。
P4'= [1，-1]
现在我们通过减去偏移矢量V来将其转换回原始坐标空间：
P4 = P4' - V = [0,0]
解决方案点P4位于预期的原点。
本文的后半部分描述了一种表示一组点的方法，其中P1不在原点或者P2不在x轴上以使它们适合这些约束。 我更愿意将它看作翻译，但两种方法都会产生相同的解决方案。
编辑：旋转P2'到X轴
如果在将P1转换为原点后P2'不在x轴上，我们必须在视图上执行旋转。
首先，我们创建一些新的向量作为示例：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]
要确定旋转角度，我们必须找到P2'和[1,0]之间的角度（x轴）。
我们可以使用点产品相等性：
A dot B = ||A|| ||B|| cos(theta)
当B是[1,0]时，这可以被简化：点B总是A的X分量，而|| B || （B的大小）总是乘以1，因此可以忽略。
我们现在有Ax = || A || cos（θ），我们可以重新排列我们的最终方程：
theta = acos(Ax / ||A||)
或者在我们的情况下：
theta = acos(P2'x / ||P2'||)
我们使用|| A ||来计算P2'的大小 = sqrt（Ax + Ay + Az）
||P2'|| = sqrt(1 + 1 + 0) = sqrt(2)
把它插入我们可以解决theta
theta = acos(1 / sqrt(2)) = 45 degrees
现在让我们使用旋转矩阵将场景旋转-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，您的中心点。
要取消旋转，请将P4“乘以R（-theta），在此情况下为R（45）。 要撤销翻译，减去偏移向量V，这与添加P1相同（假设您原先使用-P1作为V）。
这是我在3D打印机固件中使用的算法。 它避免旋转坐标系，但它可能不是最好的。
对三边测量问题有两种解决方案。 要获得第二个，请在二次方程解决方案中将“ - 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;