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

blh2enu.py

#! /usr/local/bin/python3
"""
BLH -> ENU 変換
: WGS84 の緯度(Beta)／経度(Lambda)／楕円体高(Height)を
  ENU (East/North/Up; 地平) 座標に変換する。
  * 途中、 ECEF（Earth Centered Earth Fixed; 地球中心・地球固定直交座標系）座標
    への変換を経由。

  Date          Author          Version
  2019.02.18    mk-mode.com     1.00 新規作成

Copyright(C) 2019 mk-mode.com All Rights Reserved.
---
引数: LATITUDE(BETA) LONGITUDE(LAMBDA) HEIGHT
"""
import math
import sys
import traceback


class BlhToEnu:
    USAGE = (
        "[USAGE] ./blh2enu.py BETA_O LAMBDA_O HEIGHT_O BETA LAMBDA HEIGHT\n"
        "        (BETA: LATITUDE, LAMBDA: LONGITUDE)"
    )
    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) < 7:
                print(self.USAGE)
                sys.exit(0)
            self.b_o, self.l_o, self.h_o, self.b, self.l, self.h = \
                map(lambda x: float(x), sys.argv[1:7])
        except Exception as e:
            raise

    def exec(self):
        """ Execution """
        try:
            print((
                "BLH_O: BETA = {:12.8f}°\n"
                "     LAMBDA = {:12.8f}°\n"
                "     HEIGHT = {:7.3f}m"
            ).format(self.b_o, self.l_o, self.h_o))
            print((
                "BLH:   BETA = {:12.8f}°\n"
                "     LAMBDA = {:12.8f}°\n"
                "     HEIGHT = {:7.3f}m"
            ).format(self.b, self.l, self.h))
            enu = self.__blh2enu(
                self.b_o, self.l_o, self.h_o, self.b, self.l, self.h
            )
            az = math.atan2(enu[0], enu[1]) * 180.0 / math.pi
            if az < 0.0:
                az += 360.0
            el = math.atan2(
                enu[2],
                math.sqrt(enu[0] * enu[0] + enu[1] * enu[1])
            ) * 180.0 / math.pi
            dst = math.sqrt(sum([a * a for a in enu]))
            print("--->")
            print((
                "ENU: E = {:12.3f}m\n"
                "     N = {:12.3f}m\n"
                "     U = {:12.3f}m\n"
                "方位角 = {:12.3f}°\n"
                "  仰角 = {:12.3f}°\n"
                "  距離 = {:12.3f}m"
            ).format(*enu, az, el, dst))
        except Exception as e:
            raise

    def __blh2enu(self, b_o, l_o, h_o, b, l, h):
        """ BLH -> ENU 変換(Earth, North, Up)

        :param  float b_o: 原点 Beta(Latitude)
        :param  float l_o: 原点 Lambda(Longitude)
        :param  float h_o: 原点 Height
        :param  float   b: 対象 Beta(Latitude)
        :param  float   l: 対象 Lambda(Longitude)
        :param  float   h: 対象 Height
        :return list  enu: ENU 座標 [e, n, u]
        """
        try:
            x_o, y_o, z_o = self.__blh2ecef(b_o, l_o, h_o)
            x, y, z = self.__blh2ecef(b, l, h)
            mat_0 = self.__mat_z(90.0)
            mat_1 = self.__mat_y(90.0 - b_o)
            mat_2 = self.__mat_z(l_o)
            mat = self.__mul_mat(self.__mul_mat(mat_0, mat_1), mat_2)
            return self.__rotate(mat, [x - x_o, y - y_o, z - z_o])
        except Exception as e:
            raise

    def __blh2ecef(self, lat, lon, ht):
        """ BLH -> ECEF 変換

        :param  float lat: Latitude
        :param  float lon: Longitude
        :param  float  ht: Height
        :return list     : ECEF 座標 [x, y, z]
        """
        try:
            n = lambda x: self.A / \
                math.sqrt(1.0 - self.E2 * math.sin(x * self.PI_180)**2)
            x = (n(lat) + ht) \
              * math.cos(lat * self.PI_180) \
              * math.cos(lon * self.PI_180)
            y = (n(lat) + ht) \
              * math.cos(lat * self.PI_180) \
              * math.sin(lon * self.PI_180)
            z = (n(lat) * (1.0 - self.E2) + ht) \
              * math.sin(lat * self.PI_180)
            return [x, y, z]
        except Exception as e:
            raise

    def __mat_x(self, ang):
        """ x 軸を軸とした回転行列

        :param  float ang: 回転量(°)
        :return list     : 回転行列(3x3)
        """
        try:
            a = ang * self.PI_180
            c = math.cos(a)
            s = math.sin(a)
            return [
                [1.0, 0.0, 0.0],
                [0.0,   c,   s],
                [0.0,  -s,   c]
            ]
        except Exception as e:
            raise

    def __mat_y(self, ang):
        """ y 軸を軸とした回転行列

        :param  float ang: 回転量(°)
        :return list     : 回転行列(3x3)
        """
        try:
            a = ang * self.PI_180
            c = math.cos(a)
            s = math.sin(a)
            return [
                [   c, 0.0,  -s],
                [ 0.0, 1.0, 0.0],
                [   s, 0.0,   c]
            ]
        except Exception as e:
            raise

    def __mat_z(self, ang):
        """ z 軸を軸とした回転行列

        :param  float ang: 回転量(°)
        :return list     : 回転行列(3x3)
        """
        try:
            a = ang * self.PI_180
            c = math.cos(a)
            s = math.sin(a)
            return [
                [  c,   s, 0.0],
                [ -s,   c, 0.0],
                [0.0, 0.0, 1.0]
            ]
        except Exception as e:
            raise

    def __mul_mat(self, mat_a, mat_b):
        """ 2行列(3x3)の積

        :param  list mat_a: 3x3 行列
        :param  list mat_b: 3x3 行列
        :return list      : 3x3 行列
        """
        try:
            return [
                [sum([mat_a[k][i] * mat_b[i][j] for i in range(3)
                ]) for j in range(3)] for k in range(3)
            ]
        except Exception as e:
            raise

    def __rotate(self, mat, pt):
        """ 点の回転

        :param  list mat: 3x3 回転行列
        :param  list  pt: 回転前座標 [x, y, z]
        :return list    : 回転後座標 [x, y, z]
        """
        try:
            return [
                sum([mat[j][i] * pt[i] for i in range(3)])
                for j in range(3)
            ]
        except Exception as e:
            raise


if __name__ == '__main__':
    try:
        obj = BlhToEnu()
        obj.exec()
    except Exception as e:
        traceback.print_exc()
        sys.exit(1)