Python script to convert ECEF to WGS84(BLH) coordinate.

ecef2blh.py

#! /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)