ECEF座標からENU座標に変換するプログラムです。
下記を参考にしてpythonでプログラムを作成しました。
www.enri.go.jp
pythonを使うと行列の扱いがとても楽です。
別プログラムで使用しやすいようecef2enuという関数にしています。
#WGS84 -> ENU import math import numpy as np from gps import ecef2blh #別記事 参照 from gps import blh2ecef #別記事 参照 #X軸回りのsita度の回転変換：右ねじの方向 def rotx(sita): rotx_00 = 1 rotx_01 = 0 rotx_02 = 0 rotx_10 = 0 rotx_11 = math.cos(sita*math.pi/180.0) rotx_12 = math.sin(sita*math.pi/180.0) rotx_20 = 0 rotx_21 = -math.sin(sita*math.pi/180.0) rotx_22 = math.cos(sita*math.pi/180.0) rotxa = np.array([[rotx_00,rotx_01,rotx_02], [rotx_10,rotx_11,rotx_12], [rotx_20,rotx_21,rotx_22]]) return rotxa def roty(sita): roty_00 = math.cos(sita*math.pi/180.0) roty_01 = 0 roty_02 = -math.sin(sita*math.pi/180.0) roty_10 = 0 roty_11 = 1 roty_12 = 0 roty_20 = math.sin(sita*math.pi/180.0) roty_21 = 0 roty_22 = math.cos(sita*math.pi/180.0) rotya = np.array([[roty_00,roty_01,roty_02], [roty_10,roty_11,roty_12], [roty_20,roty_21,roty_22]]) return rotya def rotz(sita): rotz_00 = math.cos(sita*math.pi/180.0) rotz_01 = math.sin(sita*math.pi/180.0) rotz_02 = 0 rotz_10 = -math.sin(sita*math.pi/180.0) rotz_11 = math.cos(sita*math.pi/180.0) rotz_12 = 0 rotz_20 = 0 rotz_21 = 0 rotz_22 = 1 rotza = np.array([[rotz_00,rotz_01,rotz_02], [rotz_10,rotz_11,rotz_12], [rotz_20,rotz_21,rotz_22]]) return rotza def ecef2enu(dest,origin): blh = ecef2blh(origin[0],origin[1],origin[2]) rotzp1 = rotz(90.0) rotyp = roty(90.0-blh[0]) rotzp2 = rotz(blh[1]) mat_conv1 = np.dot(rotzp1,rotyp) mat_conv2 = np.dot(mat_conv1,rotzp2) mov = dest - origin ret = np.dot(mat_conv2,mov) return ret
