1

目标是从已知 GPS 位置的地面站找到移动单元的位置，向移动单元报告模糊或有噪声的范围。为此，我希望脚本将从谷歌地图获取的纬度经度高度 (LLA) 转换为最小二乘算法可以使用的 XYZ 坐标，然后将 XYZ 猜测/结果返回为谷歌地图上准确的 LLA 坐标。当我运行脚本时，纬度和经度似乎相当准确，但高度错误得令人难以置信，我无法弄清楚原因。我知道 ECEF 到 LLA 的转换是基于 alt 的，所以如果我能找出它为什么如此不准确，我也可以提高纬度/经度的准确性。

有谁知道为什么高度如此错误以及我该如何解决？ 

# 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]))
4

0 回答 0