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

Википедия дает довольно подробное обсуждение алгебра здесь: http://en.wikipedia.org/wiki/Trilateration

первый шаг, на самом деле не охваченный записью Википедии, заключается в преобразовании ваших координат lat/long в Декартовые координаты:

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 в 2 компоненты: один параллельный p1a (который идет по нашей оси x) и один перпендикулярный p1a (который идет по нашей оси y в системе координат "b").

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

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

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

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