Трилатерация с использованием трех точек широты и долготы и трех расстояний.

Первый шаг, который на самом деле не описан в статье Википедии, - это преобразовать ваши координаты широты и долготы в декартовы координаты:

x0 = cos( lon0 ) * cos( lat0 ) , y0 = sin( lon0 ) * cos( lat0 ) , z0 = sin( lat0 )
x1 = cos( lon1 ) * cos( lat0 ) , y1 = sin( lon1 ) * cos( lat1 ) , z1 = sin( lat1 )
x2 = cos( lon2 ) * cos( lat0 ) , y2 = sin( lon2 ) * cos( lat2 ) , z2 = sin( lat2 )

(Чтобы не усложнять вычисления, я ошибся, поэтому мы работаем в единицах «радиусов Земли», а не в километрах)

Для ваших данных я получаю

         p0            p1           p2
X   -0.420442596  -0.420430618  -0.42040255
Y   -0.67380418   -0.673826567  -0.673825967
Z    0.607631426   0.607614889   0.607634975

Следующим шагом, который описан в статье Википедии, является упрощение координат путем перевода точек так, чтобы p0 находился в начале координат, а затем поворота так, чтобы p1 находился на оси X, а p2 находился в плоскости X-Y.

Для перевода просто вычтите p0 из p1 и p2:

    p0a      p1a          p2a
X   0    1.19779E-05   4.00462E-05
Y   0   -2.23864E-05  -2.17865E-05
Z   0   -1.65372E-05   3.5486E-06

Вращение не намного сложнее. p1b получает (x, y) = (d, 0), где d - это просто расстояние от начала координат до p1a (теорема Пифагора)

Для p2b нам нужно разделить p2a на две составляющие: одну, параллельную p1a (которая проходит по нашей оси x), и одну, перпендикулярную p1a (которая проходит по нашей оси y в системе координат "b").

Для этого нам нужен единичный вектор в направлении p1a, который равен p1a * (1 / d). Возьмите скалярное произведение этого единичного вектора (назовите его p1a_hat, если хотите) на p2a, и это координата X для p2b. В статье в Википедии это значение называется «Я».

Теперь с координатой Y легко. Длина от начала координат до p2 не может измениться при преобразовании координат. Итак, рассчитайте длину p2a, используя теорему Пифагора, а затем используйте теорему Пифагора «в обратном направлении», чтобы получить, какой должна быть координата Y для p2b, чтобы длина оставалась неизменной. Это переменная, которую Википедия называет «J». (Обратите внимание, есть двусмысленность, которую я оставлю вам, чтобы вы выяснили, является ли J положительным или отрицательным).

Теперь у вас есть три переменные d, I и J, которые в статье Википедии используются для вычислений. Теперь вы можете преобразовать их обратно в километры, умножив на радиус Земли. Отсюда вы сможете произвести остальные вычисления.

(Между прочим, Википедия дает другой расчет для преобразования координат. Мне нравится избегать триггера там, где это возможно).