trilat_linalg.py 文件源码

def trilaterate(points):
    LatA = points[0][1]
    LonA = points[0][0]
    DistA = points[0][2]*MagicDistanceFactor
    LatB = points[1][1]
    LonB = points[1][0]
    DistB = points[1][2]*MagicDistanceFactor
    LatC = points[2][1]
    LonC = points[2][0]
    DistC = points[2][2]*MagicDistanceFactor

    # Transform from Latitude/Longitude to ECEF coordinates.
    xA,yA,zA = pyproj.transform(lla, ecef, LonA, LatA, 0, radians=False)
    xB,yB,zB = pyproj.transform(lla, ecef, LonB, LatB, 0, radians=False)
    xC,yC,zC = pyproj.transform(lla, ecef, LonC, LatC, 0, radians=False)

    # Convert to numpy arrays.
    P1 = numpy.array([xA/1000.0, yA/1000.0, zA/1000.0])
    P2 = numpy.array([xB/1000.0, yB/1000.0, zB/1000.0])
    P3 = numpy.array([xC/1000.0, yC/1000.0, zC/1000.0])

    # Sphere intersection from Wikipedia + Stackoverflow.
    ex = (P2 - P1)/(numpy.linalg.norm(P2 - P1))
    i = numpy.dot(ex, P3 - P1)
    ey = (P3 - P1 - i*ex)/(numpy.linalg.norm(P3 - P1 - i*ex))
    ez = numpy.cross(ex,ey)
    d = numpy.linalg.norm(P2 - P1)
    j = numpy.dot(ey, P3 - P1)
    x = (pow(DistA,2) - pow(DistB,2) + pow(d,2))/(2*d)
    y = ((pow(DistA,2) - pow(DistC,2) + pow(i,2) + pow(j,2))/(2*j)) - ((i/j)*x)

    # Handle errors.
    if pow(DistA,2) - pow(x,2) - pow(y,2) < 0:
        return []
    z = numpy.sqrt(pow(DistA,2) - pow(x,2) - pow(y,2))

    lon,lat,altitude = pyproj.transform(ecef, lla, x*1000,y*1000,z*1000, radians=False)
    lon,lat,altitude = pyproj.transform(ecef, lla, x*1000,y*1000,z*1000, radians=True)

    triPt = P1 + x*ex + y*ey + z*ez
    lon,lat,altitude = pyproj.transform(ecef, lla, triPt[0]*1000,triPt[1]*1000,triPt[2]*1000, radians=False)

    return [lat,lon]


# This was copied from trilat_optproblem.py and changed for only three point
# combinations.