BLU coord to XYZ coord.

BLH2XYZ.py

def BLH2XYZ(B, L, H):
    '''B: lat  L:lon  H: height'''
    Lat, Lon = B, L

    N, E, h = 0, 0, 0
    L0 = (int((L - 1.5) / 3.0) + 1) * 3.0  #

    a = 6378245.0  #
    F = 298.257223563  #
    iPI = 0.0174532925199433  #

    f = 1 / F
    b = a * (1 - f)
    ee = (a * a - b * b) / (a * a)
    e2 = (a * a - b * b) / (b * b)
    n = (a - b) / (a + b)
    n2 = (n * n)
    n3 = (n2 * n)
    n4 = (n2 * n2)
    n5 = (n4 * n)
    al = (a + b) * (1 + n2 / 4 + n4 / 64) / 2
    bt = -3 * n / 2 + 9 * n3 / 16 - 3 * n5 / 32
    gm = 15 * n2 / 16 - 15 * n4 / 32
    dt = -35 * n3 / 48 + 105 * n5 / 256
    ep = 315 * n4 / 512

    B = B * iPI
    L = L * iPI
    L0 = L0 * iPI
    l = L - L0
    cl = (math.cos(B) * l)
    cl2 = (cl * cl)
    cl3 = (cl2 * cl)
    cl4 = (cl2 * cl2)
    cl5 = (cl4 * cl)
    cl6 = (cl5 * cl)
    cl7 = (cl6 * cl)
    cl8 = (cl4 * cl4)

    lB = al * (B + bt * math.sin(2 * B) + gm * math.sin(4 * B) + dt * math.sin(6 * B) + ep * math.sin(8 * B))
    t = math.tan(B)
    t2 = (t * t)
    t4 = (t2 * t2)
    t6 = (t4 * t2)
    Nn = a / math.sqrt(1 - ee * math.sin(B) * math.sin(B))
    yt = e2 * math.cos(B) * math.cos(B)
    N = lB
    N += t * Nn * cl2 / 2
    N += t * Nn * cl4 * (5 - t2 + 9 * yt + 4 * yt * yt) / 24
    N += t * Nn * cl6 * (61 - 58 * t2 + t4 + 270 * yt - 330 * t2 * yt) / 720
    N += t * Nn * cl8 * (1385 - 3111 * t2 + 543 * t4 - t6) / 40320

    E = Nn * cl
    E += Nn * cl3 * (1 - t2 + yt) / 6
    E += Nn * cl5 * (5 - 18 * t2 + t4 + 14 * yt - 58 * t2 * yt) / 120
    E += Nn * cl7 * (61 - 479 * t2 + 179 * t4 - t6) / 5040

    E += 500000

    N = 0.9999 * N
    E = 0.9999 * (E - 500000.0) + 250000.0

    return E, N  # x,y