1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
| #! /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
|