緯度経度(WGS84)ーECEF座標変換プログラム

緯度経度→ECEF座標系に変換する。
ECEF座標系→緯度経度に変換するプログラム

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

#東京タワー
p_deg = 35.658581 #[deg]
l_deg = 139.745433 #[deg]
h = 333.0 #[m]

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

pi = 3.1415926535898 #円周率
a = 6378137.0 #赤道面平均半径
ONE_F = 298.257223563 #扁平率の逆数

E2 = ((1.0/ONE_F)*(2-(1.0/ONE_F)))
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)

print("x = " + str(x))
print("y = " + str(y))
print("z = " + str(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)
h2 = (p/math.cos(p_rad2)) - NN

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

print("φ = " + str(p_deg2))
print("λ = " + str(l_deg2))
print("ｈ = " + str(h2))