在Python中,最小二乘法给出了错误的海拔高度

2024-06-14 07:15:24 发布

您现在位置:Python中文网/ 问答频道 /正文

目标是从已知GPS位置的地面站找到移动单元的位置,向移动单元报告模糊或有噪声的范围。为此,我希望脚本将从Google地图获取的经纬度高度(LLA)转换为可由最小二乘算法使用的XYZ坐标,然后将XYZ猜测/结果返回Google地图上精确的LLA坐标。当我运行脚本时,纬度和经度看起来相当准确,但是海拔高度是难以置信的错误,我还不知道为什么。我知道ECEF到LLA的转换是基于alt的,所以如果我能找出它如此不准确的原因,我也可以提高lat/lon的精确度。在

有人知道为什么海拔高度这么高吗?我怎么能修正它?在

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

Tags: toinnumpycoordsalttmplonlat