Я хочу, чтобы найти координаты неизвестной node которые лежат где-то в пространстве, которое имеет свое эталонное расстояние от 3-й или более узлов, все они известны координаты.
Эта проблема в точности подобна Trilateration, как описано здесь Trilateration.
Однако я не понимаю часть "Предварительные и окончательные вычисления" (см. сайт wikipedia). Я не понимаю, где я мог бы найти P1, P2 и P3, чтобы я мог поставить эти уравнения?
Спасибо
Trilateration - это процесс нахождения центра области пересечения трех сфер. Центральная точка и радиус каждой из трех сфер должны быть известны.
Рассмотрим ваши три примера центральных точек 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]
Чтобы определить угол поворота, мы должны найти угол между P2 'и [1,0] (ось x).
Мы можем использовать dot product:
A dot B = ||A|| ||B|| cos(theta)
Когда B является [1,0], это можно упростить: точка B всегда является только X-компонентой A, а || B || (величина B) всегда умножается на 1 и поэтому может быть проигнорирована.
Теперь имеем Ax = || A || cos (theta), которую мы можем переставить к нашему окончательному уравнению:
theta = acos(Ax / ||A||)
или в нашем случае:
theta = acos(P2'x / ||P2'||)
Вычислим величину P2 ', используя || A || = sqrt (Ax + Ay + Az)
||P2'|| = sqrt(1 + 1 + 0) = sqrt(2)
Подключив это, мы можем решить для thetap >
theta = acos(1 / sqrt(2)) = 45 degrees
Теперь позвольте использовать матрицу вращения , чтобы повернуть сцену на -45 градусов. Поскольку P2'y положительно, а матрица вращения вращается против часовой стрелки, мы будем использовать отрицательное вращение, чтобы выровнять P2 с осью x (если P2'y отрицательно, не отрицайте theta).
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-принтера. Он избегает поворота системы координат, но может быть не лучшим.
Существует 2 решения проблемы трилатерации. Чтобы получить второй, замените "- sqrtf" на "+ sqrtf" в решении квадратичного уравнения.
Очевидно, вы можете использовать удвоители вместо float, если у вас достаточно мощности процессора и памяти.
// 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;