Мультилатерация GPS-координат

Question

У меня есть N GPS-координаты с N расстояниями, заданными в неизвестном положении, которое я хочу определить.

Мой первый подход состоял в том, чтобы использовать только три точки и трилатерацию, точно так, как описано здесь . Этот подход был уже достаточно точным (лучшая ошибка ~ 5 км), но я хотел бы улучшить его и повысить надежность. Поскольку данные расстояния не очень точны с самого начала, я подумал об использовании нескольких измерений и мультилатерации. Однако оказалось, что этот подход гораздо менее точен (лучшая ошибка ~ 100 км), хотя я предоставляю более 3 точек / расстояний (проверено до 6), и теперь я спрашиваю, есть ли у кого-то идея, что я мог бы иметь сделано неправильно.

Короче говоря, мой подход к мультилатерации следующий:

1. Конвертировать все координаты в ECEF
2. Построить матрицу, как описано в уравнении 7 в wikipedia
3. Используйте SVD, чтобы найти минимизатор
4. Поскольку решение является только масштабируемым, я использую метод нахождения корня, чтобы определить нормализацию, чтобы координаты, преобразованные обратно в LLA, дали высоту 0 (мое первоначальное предположение состоит в том, что все координаты находятся на нулевой высоте)
5. Конвертировать обратно в LLA

LLA / ECEF-преобразование проверено и проверено дважды. Шаг 2 и 3 Я проверил евклидовы координаты (и точные расстояния) и выглядел правильно. Я сам выполнил шаг 4, я понятия не имею, если это вообще хороший подход, поэтому предложения приветствуются.

+++ UPDATE

Я собрал пример кода на python, чтобы проиллюстрировать проблему с некоторой земной правдой. Трилатерация достигает 400 метров, а Диапазон мультилатерации здесь 10-130км. Из-за длины я поставил ideone

Answer 1

В конце концов, я понял это сам - или, по крайней мере, значительно улучшил точность.

Подход, описанный в Википедии (уравнение 7), очевидно, не очень подходит для этого приложения, но в этом случае он уже намного проще.

Учитывая уравнение 6 из Википедии, мы можем сильно упростить это: R_0 можно угадать как радиус Земли, так как начало координат ECEF лежит в центре Земли. Следовательно, нет необходимости сдвигать все, чтобы сделать одну точку источником, и мы можем использовать все N уравнения.

В python, с P массивом координат ECEF и dists расстояниями до этих точек, все сводится к

R = 6378137 # Earth radius in meters
A = []
for m in range(0,len(P)):
    x = P[m][0]
    y = P[m][1]
    z = P[m][2]
    Am = -2*x
    Bm = -2*y
    Cm = -2*z
    Dm = R*R + (pow(x,2)+pow(y,2)+pow(z,2)) - pow(dists[m],2)
    A += [[Am,Bm,Cm,Dm]]
# Solve using SVD
A = numpy.array(A)
(_,_,v) = numpy.linalg.svd(A)
# Get the minimizer
w = v[3,:]
w /= w[3] # Resulting position in ECEF

При таком подходе то, что я описал как Шаг 4 , больше не требуется. Фактически, это даже ухудшает решение.

Теперь точность колеблется между 2 км и 275 м - в большинстве случаев лучше, чем «оптимальная» трилатерация с ошибкой 464 м.

Answer 2

Некоторые комментарии:

1) Вы уже проверили некоторые шаги с точными ответами.Я предлагаю вам создать игрушечные проблемы с известным количеством случайного шума, добавленного к наблюдениям.Поскольку вы знаете правильный ответ в этом случае, вы можете увидеть, что происходит с распространением ошибок.Если ваш метод хорошо работает здесь, но плохо работает с реальными данными, вы можете подумать об ужасном поведении в реальной жизни, например, о том, что одно или несколько расстояний серьезно ошибочно.

2) Я не знаю, почемуВаше решение является только масштабируемым, поскольку базовые данные правильно масштабируются - если бы я пошел туда с отрезанными по длине веревками и привязал их к фиксированным точкам, не было бы никакой двусмысленности.Когда вы используете SVD для решения уравнений (7), вы делаете что-то вроде www.cse.unr.edu/~bebis/MathMethods/SVD/lecture.pdf, чтобы получить решение наименьших квадратов?Это должно дать вам x, y и z без двусмысленности.

3) Я совсем не уверен в том, как ошибки наблюдений работают через (7).Я не люблю все подразделения, с одной стороны.Возможно, стоит записать уравнение для суммы квадратов разностей между измеренными расстояниями и вычисленными расстояниями, заданными x, y, z для неизвестного положения, а затем минимизировать это для x, y, z.Статья в Википедии отказывается от этого подхода из-за его стоимости, но он может дать вам более точный ответ, а вычисление и сравнение этого ответа может вам кое-что сказать, даже если вы не можете использовать этот метод на практике.

Answer 3

Чтобы улучшить принятый ответ , один из способов улучшить решение SVD - учесть изменение радиуса Земли по широте; это особенно влияет на оценку высоты, но также оказывает влияние на широту и долготу. «Простым» решением было бы использовать среднее значение для R, которое согласно Википедии составляет 6371008,8 м, а не 6378137 м.

Более точной оценкой будет корректировка R для широты:

def EarthRadiusAtLatitude(lat):
    rlat = np.deg2rad(lat)

    a = np.float64(6378137.0)
    b = np.float64(6356752.3)

    rad = np.sqrt(((a*a*np.cos(rlat))**2 + (b*b*np.sin(rlat))**2) /
                  ((a*np.cos(rlat))**2 + (b*np.sin(rlat))**2))
    return rad

Затем установите R на основе широты одной из начальных точек. Или, если у вас большой разброс по широте, вы можете вычислить SVD на основе оценки R и использовать предварительную широту решения для решения, используя более близкую оценку R.

После выполнения этой корректировки в моих экспериментах с построенными примерами и данными "реального мира", основанными на значениях опережения синхронизации LTE eNodeB, решение SVD обычно находится в пределах одной секунды от широты и долготы, за исключением некоторых вырожденных случаев, что довольно сравнимо с решением, основанным на итеративной оптимизации (т. е. минимизации остатков расстояния).

Answer 4

Я следовал приведенному выше коду @ zerm, и он работал довольно хорошо (желтый - это точка, рассчитанная по 3 башням).Результаты показаны во фрагменте Фолиума. Мультилатерация с использованием linalg.SVD

Однако, когда я следовал тому же алгоритму с изменениями, предложенными @mcdowella (# 2), используя наименьшие квадраты системного решения MxN, результаты были намного лучше, Мультилатерация с использованием наименьших квадратов MxN

Вот исправленный код:

A = []
b = []
for m in range(0,len(P)):
    x = P[m][0]
    y = P[m][1]
    z = P[m][2]
    Am = 2*x
    Bm = 2*y
    Cm = 2*z
    Dm = R*R + (pow(x,2)+pow(y,2)+pow(z,2)) - pow(dists[m],2)
    A += [[Am,Bm,Cm]]
    b += [[Dm]]
# Solve using Least Squares of an MxN System
# A*x = b --> x = (ATA)_inv.AT.b = A+.b
A = np.array(A)
b = np.array(b)
AT = A.T
ATA = np.matmul(AT,A)
ATA_inv = np.linalg.inv(ATA)
Aplus = np.matmul(ATA_inv,AT)
x = np.matmul(Aplus,b)
# convert back to lat/long from ECEF
# convert to degrees
lat = math.degrees(math.asin(x[2] / R))
lon = math.degrees(math.atan2(x[1],x[0]))

Я все еще изучаю другие методы мультилатерации, но этот пост действительно позволил мне понять основыиз N-баллов MLAT.Спасибо!