Наименьших квадратов дает неправильную высоту с LLA для ECEF для LLA в Python - PullRequest
Новая
       80

Наименьших квадратов дает неправильную высоту с LLA для ECEF для LLA в Python

0 голосов
/ 10 июня 2018

Цель состоит в том, чтобы найти местоположение мобильного устройства от наземных станций в известных местоположениях GPS, сообщая о дальностях мобильного устройства, которые являются нечеткими или имеют шум.Чтобы сделать это, я хочу, чтобы скрипт преобразовал широту и долготу (LLA), взятые из Карт Google, в координаты XYZ, которые могут использоваться алгоритмом наименьших квадратов, а затем возвратил предположение / результат XYZ обратно как координаты LLA, точные на картах Google.Лат и Лон кажутся довольно точными, когда я запускаю сценарий, но высота невероятно неправильная, и я не смог понять, почему.Я знаю, что преобразование ECEF в LLA основано на альте, поэтому, если бы я смог выяснить, почему оно так неточно, я мог бы также улучшить точность широты и долготы.

Кто-нибудь знает, почему высотаэто так неправильно и как я могу это исправить?

# Hello World program in Python
import numpy
import pyproj

miles = 0

# Lat, Long, Alt in m above sea level, Range in m
coords = [
  (36.8932303,  -76.3019194,  2, 1609.3440006146922),
  (36.90476155,  -76.19754928, 5, 8257),
  (36.87400768,  -76.2566008,  3, 2962),
  (36.8762048,  -76.18793625,  7, 8836),
  (36.88499263,  -76.41727585,  4, 11000),
]

NumRefs = len(coords)

ecef = pyproj.Proj(proj='geocent', ellps='WGS84', datum='WGS84')
lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84')

def lla_to_ecef(lat, lon, alt):
    x, y, z = pyproj.transform(lla, ecef, lon, lat, alt, radians=False)
    return x, y, z

def ecef_to_lla(x, y, z):
    lon, lat, alt = pyproj.transform(ecef, lla, x, y, z, radians=False)
    return lat, lon, alt

RefLoc = numpy.zeros((NumRefs,3))
RefLoc = numpy.asmatrix(RefLoc)
Ranges = numpy.zeros((NumRefs,1))
Ranges = numpy.asmatrix(Ranges)

for x in range(0, NumRefs):
    Tmp = lla_to_ecef(coords[x][0],coords[x][1],coords[x][2])
    RefLoc[x,0] = Tmp[0]
    RefLoc[x,1] = Tmp[1]
    RefLoc[x,2] = Tmp[2]
    #print(ecef_to_lla(Tmp[0], Tmp[1], Tmp[2]))
    if miles == 0:
       Ranges[x] = coords[x][3]
    else:
       Ranges[x] = coords[x][3] * 1609.344

#code to help debug
#RefLoc = numpy.mat("-51.295,23.2089,9.9447;85.8527,-5.3422,83.4387;-30.0032,-29.6681,-42.8322;-60.6809,66.1657,51.44;-49.7832,17.0528,50.7458")
#Ranges = numpy.mat("99.1211;201.7909;60.5797;150.6262;138.0656")

H = numpy.zeros((NumRefs,4))
H = numpy.asmatrix(H)
for x in range(0, NumRefs):
    H[x,0] = 1
    H[x,1] = -2*RefLoc[x,0]
    H[x,2] = -2*RefLoc[x,1]
    H[x,3] = -2*RefLoc[x,2]

b = numpy.zeros((NumRefs,1))
b = numpy.asmatrix(b)
for x in range(0, NumRefs):
     b[x,0] = Ranges[x]**2 - RefLoc[x,0]**2 - RefLoc[x,1]**2 - RefLoc[x,2]**2 

xHat = numpy.linalg.inv(numpy.transpose(H)*H)*numpy.transpose(H)*b
xyzEst = [xHat[1],xHat[2],xHat[3]]
#print(xyzEst)

print(ecef_to_lla(xHat[1], xHat[2], xHat[3]))
...