Skip to content

Instantly share code, notes, and snippets.

@komasaru komasaru/blh2enu.rb
Created Feb 24, 2019

Embed
What would you like to do?
Ruby script to convert WGS84(BLH) to ENU coordinate.
#! /usr/local/bin/ruby
#---------------------------------------------------------------------------------
# BLH -> ENU 変換
# : WGS84 の緯度(Beta)/経度(Lambda)/楕円体高(Height)を
# ENU (East/North/Up; 地平) 座標に変換する。
# * 途中、 ECEF(Earth Centered Earth Fixed; 地球中心・地球固定直交座標系)座標
# への変換を経由。
#
# Date Author Version
# 2019.02.20 mk-mode.com 1.00 新規作成
#
# Copyright(C) 2019 mk-mode.com All Rights Reserved.
#---------------------------------------------------------------------------------
#
class ConvBlh2Enu
USAGE = "[USAGE] ./blh2enu.rb BETA_O LAMBDA_O HEIGHT_O BETA LAMBDA HEIGHT\n"
USAGE << " (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
# Initialization
# * コマンドライン引数の取得
def initialize
if ARGV.size < 6
puts USAGE
exit
end
@blh_o = ARGV[0, 3].map(&:to_f)
@blh = ARGV[3, 3].map(&:to_f)
end
# Execution
def exec
puts "BLH_O: LATITUDE(BETA) = %12.8f°" % @blh_o[0]
puts " LONGITUDE(LAMBDA) = %12.8f°" % @blh_o[1]
puts " HEIGHT = %12.3fm" % @blh_o[2]
puts "BLH: LATITUDE(BETA) = %12.8f°" % @blh[0]
puts " LONGITUDE(LAMBDA) = %12.8f°" % @blh[1]
puts " HEIGHT = %12.3fm" % @blh[2]
enu = blh2enu(@blh_o, @blh)
az = Math.atan2(enu[0], enu[1]) / PI_180
az += 360.0 if az < 0.0
el = Math.atan2(
enu[2],
Math.sqrt(enu[0] * enu[0] + enu[1] * enu[1])
) / PI_180
dst = Math.sqrt(enu.map { |a| a * a }.inject(:+))
puts "--->"
puts "ENU: E = %12.3fm" % enu[0]
puts " N = %12.3fm" % enu[1]
puts " U = %12.3fm" % enu[2]
puts "方位角 = %12.3f°" % az
puts " 仰角 = %12.3f°" % el
puts " 距離 = %12.3fm" % dst
rescue => e
msg = "[#{e.class}] #{e.message}\n"
e.backtrace.each { |tr| msg << "\t#{tr}\n" }
$stderr.puts msg
exit 1
end
private
# BLH -> ENU 変換(Earth, North, Up)
#
# @param blh_o: BLH 座標(原点) [latitude, longitude, height]
# @param blh: BLH 座標(対象) [latitude, longitude, height]
# @return enu: ENU 座標 [e, n, u]
def blh2enu(blh_o, blh)
x_o, y_o, z_o = blh2ecef(blh_o)
x, y, z = blh2ecef(blh)
mat_0 = mat_z(90.0)
mat_1 = mat_y(90.0 - blh_o[0])
mat_2 = mat_z(blh_o[1])
mat = mul_mat(mul_mat(mat_0, mat_1), mat_2)
return rotate(mat, [x - x_o, y - y_o, z - z_o])
rescue => e
raise
end
# BLH -> ECEF 変換
#
# @param blh: BLH 座標 [latitude, longitude, height]
# @return ecef: ECEF 座標 [x, y, z]
def blh2ecef(blh)
lat, lon, ht = blh
n = lambda do |x|
s = Math.sin(x * PI_180)
A / Math.sqrt(1.0 - E2 * s * s)
end
c_lat = Math.cos(lat * PI_180)
c_lon = Math.cos(lon * PI_180)
s_lat = Math.sin(lat * PI_180)
s_lon = Math.sin(lon * PI_180)
x = (n.call(lat) + ht) * c_lat * c_lon
y = (n.call(lat) + ht) * c_lat * s_lon
z = (n.call(lat) * (1.0 - E2) + ht) * s_lat
return [x, y, z]
rescue => e
raise
end
# x 軸を軸とした回転行列
#
# @param ang: 回転量(°)
# @return : 回転行列(3x3)
def mat_x(ang)
a = ang * 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]
]
rescue => e
raise
end
# y 軸を軸とした回転行列
#
# @param ang: 回転量(°)
# @return : 回転行列(3x3)
def mat_y(ang)
a = ang * 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]
]
rescue => e
raise
end
# z 軸を軸とした回転行列
#
# @param ang: 回転量(°)
# @return : 回転行列(3x3)
def mat_z(ang)
a = ang * 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]
]
rescue => e
raise
end
# 2行列(3x3)の積
#
# @param mat_a: 3x3 行列
# @param mat_b: 3x3 行列
# @return mat: 3x3 行列
def mul_mat(mat_a, mat_b)
mat = Array.new(3).map { |a| Array.new(3, 0.0) }
0.upto(2) do |i|
0.upto(2) do |j|
0.upto(2) do |k|
mat[i][j] += mat_a[i][k] * mat_b[k][j]
end
end
end
return mat
rescue => e
raise
end
# 点の回転
#
# @param mat_r: 3x3 回転行列
# @param pt: 回転前座標 [x, y, z]
# @return mat: 回転後座標 [x, y, z]
def rotate(mat_r, pt)
mat = Array.new(3, 0.0)
0.upto(2) do |i|
0.upto(2) do |j|
mat[i] += mat_r[i][j] * pt[j]
end
end
return mat
rescue => e
raise
end
end
ConvBlh2Enu.new.exec if __FILE__ == $0
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.