緯度経度からECEF座標系変換

下記を参考にしてECEF座標系から緯度経度、その逆を変換するpythonプログラムを
作成したので、備忘録を兼ねて記載しておきます。
www.enri.go.jp

汎用的に使えるよう、それぞれ関数にしておきました。

#緯度経度からECEF座標系を算出する
import math
import numpy as np

#lat = 38.13579617 #[deg]
#lon = 140.91581617 #[deg]
#hig = 41.940 #[m]

pi = 3.1415926535898 #円周率
a = 6378137.0 #赤道面平均半径
ONE_F = 298.257223563 #扁平率の逆数
E2 = ((1.0/ONE_F)*(2-(1.0/ONE_F)))

#----------------------------------
def blh2ecef(p_deg,l_deg,h):

    p_rad = math.radians(p_deg)
    l_rad = math.radians(l_deg)

    NN = (a / math.sqrt(1.0 - (E2)*(math.sin(p_rad)**2)))

    x = (NN+h)*math.cos(p_rad)*math.cos(l_rad)
    y = (NN+h)*math.cos(p_rad)*math.sin(l_rad)
    z = (NN*(1-E2)+h)*math.sin(p_rad)

    return np.array([x,y,z])

#ecef = blh2ecef(lat,lon,hig)
#print(ecef)

#x2 = ecef[0]
#y2 = ecef[1]
#z2 = ecef[2]

#--------------------------------------
def ecef2blh(x,y,z):

    b = (a*(1.0 - 1.0/ONE_F))
    ED2 = (E2*a*a/(b*b))
    p = math.sqrt(x**2 + y**2)
    si = math.atan2(z*a,p*b)
    

    p_rad2 = math.atan2((z + ED2*b*(math.sin(si)**3)),(p-E2*a*(math.cos(si)**3)))
    l_rad2 = math.atan2(y,x)

    NN = (a / math.sqrt(1.0 - (E2)*(math.sin(p_rad2)**2)))
    h2 = (p/math.cos(p_rad2)) - NN

    p_deg2 = math.degrees(p_rad2)
    l_deg2 = math.degrees(l_rad2)

    return np.array([p_deg2,l_deg2,h2])

#blh = ecef2blh(x2,y2,z2)
#print(blh)