Skip to content

Instantly share code, notes, and snippets.

@komasaru komasaru/ecef2blh.py
Created Jun 29, 2018

Embed
What would you like to do?
Python script to convert ECEF to WGS84(BLH) coordinate.
#! /usr/local/bin/python3.6
"""
ECEF -> BLH 変換
: ECEF(Earth Centered Earth Fixed; 地球中心・地球固定直交座標系)座標を
WGS84 の緯度(Latitude)/経度(Longitude)/楕円体高(Height)に変換する。
Date Author Version
2018.06.28 mk-mode.com 1.00 新規作成
Copyright(C) 2018 mk-mode.com All Rights Reserved.
---
引数: X Y Z
"""
import math
import sys
import traceback
class EcefToBlh:
USAGE = "[USAGE] ./ecef2blh.py X Y Z"
PI_180 = math.pi / 180.0
# WGS84 座標パラメータ
A = 6378137.0 # a(地球楕円体長半径(赤道面平均半径))
ONE_F = 298.257223563 # 1 / f(地球楕円体扁平率=(a - b) / a)
B = A * (1.0 - 1.0 / ONE_F) # b(地球楕円体短半径)
E2 = (1.0 / ONE_F) * (2 - (1.0 / ONE_F))
# e^2 = 2 * f - f * f
# = (a^2 - b^2) / a^2
ED2 = E2 * A * A / (B * B) # e'^2= (a^2 - b^2) / b^2
def __init__(self):
""" Initialization
: コマンドライン引数の取得
"""
try:
if len(sys.argv) < 4:
print(self.USAGE)
sys.exit(0)
self.x, self.y, self.z = \
map(lambda x: float(x), sys.argv[1:4])
except Exception as e:
raise
def exec(self):
""" Execution """
try:
print((
"ECEF: X = {:12.3f}m\n"
" Y = {:12.3f}m\n"
" Z = {:12.3f}m"
).format(self.x, self.y, self.z))
lat, lon, ht = self.__ecef2blh(self.x, self.y, self.z)
print("--->")
print((
"BLH: LATITUDE(BETA) = {:12.8f}°\n"
" LONGITUDE(LAMBDA) = {:12.8f}°\n"
" HEIGHT = {:7.3f}m"
).format(lat, lon, ht))
except Exception as e:
raise
def __ecef2blh(self, x, y, z):
""" ECEF -> BLH 変換
:param float lat: X
:param float lon: Y
:param float ht: Z
:return list blh: BLH Coordinate [latitude, longitude, height]
"""
try:
n = lambda x: self.A / \
math.sqrt(1.0 - self.E2 * math.sin(x * self.PI_180)**2)
p = math.sqrt(x * x + y * y)
theta = math.atan2(z * self.A, p * self.B) / self.PI_180
lat = math.atan2(
z + self.ED2 * self.B * math.sin(theta * self.PI_180)**3,
p - self.E2 * self.A * math.cos(theta * self.PI_180)**3
) / self.PI_180
lon = math.atan2(y, x) / self.PI_180
ht = (p / math.cos(lat * self.PI_180)) - n(lat)
return [lat, lon, ht]
except Exception as e:
raise
if __name__ == '__main__':
try:
obj = EcefToBlh()
obj.exec()
except Exception as e:
traceback.print_exc()
sys.exit(1)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.