C++ - WGS84 (BLH) 座標 -> ENU 座標 変換!

Updated:


前々回と前回、 BLH 座標(WGS84 の緯度(Beta)/経度(Lambda)/楕円体高(Height))から ECEF(Earth Centered Earth Fixed; 地球中心・地球固定直交座標系)座標への変換や、その逆の変換の処理を C++ で実装しました。

今回は BLH 座標から ENU 座標(地平座標; EastNorthUp)への変換処理を C++ で実装してみました。

0. 前提条件

  • Debian GNU/Linux 10.8 (64bit) での作業を想定。
  • GCC 10.2.0 (G++ 10.2.0) (C++17) でのコンパイルを想定。
  • ここでは、 WGS84(World Geodetic System 1984) 測地系や ECEF(Earth Centered Earth Fixed; 地球中心・地球固定直交座標系)の詳細についての説明はしない。

1. ソースコードの作成

File: blh2enu.cpp

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
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
/***********************************************************
  BLH -> ENU 変換
  : WGS84 の緯度(Beta)/経度(Lambda)/楕円体高(Height)を
    ENU (East/North/Up; 地平) 座標に変換する。
    * 途中、 ECEF(Earth Centered Earth Fixed; 地球中心・地
      球固定直交座標系)座標への変換を経由。

    DATE        AUTHOR       VERSION
    2021.05.06  mk-mode.com  1.00 新規作成

  Copyright(C) 2021 mk-mode.com All Rights Reserved.

  ----------------------------------------------------------
  引数 : B_0 L_0 H_0 B_1 L_1 H_1
         * B_0, L_0, H_0: 基準の BLH(WGS84) 座標
         * B_1, L_1, H_1: 対象の BLH(WGS84) 座標
  ----------------------------------------------------------
  $ g++102 -std=c++17 -Wall -O2 --pedantic-errors -o blh2enu blh2enu.cpp
***********************************************************/
#include <cmath>
#include <cstdlib>   // for EXIT_XXXX
#include <iomanip>
#include <iostream>
#include <vector>

namespace blh2enu {

// 定数
static constexpr double kPi    = atan(1.0) * 4.0;
static constexpr double kPi180 = kPi / 180;
// [ WGS84 座標パラメータ ]
// a(地球楕円体長半径(赤道面平均半径))
static constexpr double kA     = 6378137.0;
// 1 / f(地球楕円体扁平率=(a - b) / a)
static constexpr double k1F    = 298.257223563;
// b(地球楕円体短半径)
static constexpr double kB     = kA * (1.0 - 1.0 / k1F);
// e^2 = 2 * f - f * f
//     = (a^2 - b^2) / a^2
static constexpr double kE2    = (1.0 / k1F) * (2.0 - (1.0 / k1F));
// e'^2= (a^2 - b^2) / b^2
static constexpr double kEd2   = kE2 * kA * kA / (kB * kB);

// 座標
struct CoordB {
  double b;  // B(Beta)
  double l;  // L(Lambda)
  double h;  // H(Height)
};
struct CoordX {
  double x;  // X
  double y;  // Y
  double z;  // Z
};
struct CoordE {
  double e;  // E(East)
  double n;  // N(North)
  double u;  // U(Up)
};

/*
 * @brief      x 軸を軸とした回転行列
 *
 * @param[in]  回転量(°) (double)
 * @return     回転行列(3x3) (vector<vector<double>>)
 */
std::vector<std::vector<double>> mat_x(double ang) {
  double a;
  double c;
  double s;
  std::vector<std::vector<double>> mat(3, std::vector<double>(3, 0.0));

  try {
    a = ang * kPi180;
    c = cos(a);
    s = sin(a);
    mat[0][0] = 1.0;
    mat[1][1] =   c;
    mat[1][2] =   s;
    mat[2][1] =  -s;
    mat[2][2] =   c;
  } catch (...) {
    throw;
  }

  return mat;
}

/*
 * @brief      y 軸を軸とした回転行列
 *
 * @param[in]  回転量(°) (double)
 * @return     回転行列(3x3) (vector<vector<double>>)
 */
std::vector<std::vector<double>> mat_y(double ang) {
  double a;
  double c;
  double s;
  std::vector<std::vector<double>> mat(3, std::vector<double>(3, 0.0));

  try {
    a = ang * kPi180;
    c = cos(a);
    s = sin(a);
    mat[0][0] =   c;
    mat[0][2] =  -s;
    mat[1][1] = 1.0;
    mat[2][0] =   s;
    mat[2][2] =   c;
  } catch (...) {
    throw;
  }

  return mat;
}

/*
 * @brief      z 軸を軸とした回転行列
 *
 * @param[in]  回転量(°) (double)
 * @return     回転行列(3x3) (vector<vector<double>>)
 */
std::vector<std::vector<double>> mat_z(double ang) {
  double a;
  double c;
  double s;
  std::vector<std::vector<double>> mat(3, std::vector<double>(3, 0.0));

  try {
    a = ang * kPi180;
    c = cos(a);
    s = sin(a);
    mat[0][0] =   c;
    mat[0][1] =   s;
    mat[1][0] =  -s;
    mat[1][1] =   c;
    mat[2][2] = 1.0;
  } catch (...) {
    throw;
  }

  return mat;
}

/*
 * @brief      2行列(3x3)の積
 *
 * @param[in]  元の 3x3 行列 (vector<vector<double>>)
 * @param[in]  元の 3x3 行列 (vector<vector<double>>)
 * @return     計算後の 3x3 行列 (vector<vector<double>>)
 */
std::vector<std::vector<double>> mul_mat(
    std::vector<std::vector<double>> mat_a,
    std::vector<std::vector<double>> mat_b) {
  unsigned int i;
  unsigned int j;
  unsigned int k;
  std::vector<std::vector<double>> mat(3, std::vector<double>(3, 0.0));

  try {
    for (i = 0; i < 3; ++i) {
      for (j = 0; j < 3; ++j) {
        for (k = 0; k < 3; ++k) {
          mat[i][j] += mat_a[i][k] * mat_b[k][j];
        }
      }
    }
  } catch (...) {
    throw;
  }

  return mat;
}

/*
 * @brief      座標の回転
 *
 * @param[in]  3x3 回転行列 (vector<vector<double>>)
 * @param[in]  回転前座標 (CoordX)
 * @return     回転後座標 (CoordE)
 */
CoordE rotate(std::vector<std::vector<double>> mat_r, CoordX pt_0) {
  CoordE pt;

  try {
    pt.e = mat_r[0][0] * pt_0.x
         + mat_r[0][1] * pt_0.y
         + mat_r[0][2] * pt_0.z;
    pt.n = mat_r[1][0] * pt_0.x
         + mat_r[1][1] * pt_0.y
         + mat_r[1][2] * pt_0.z;
    pt.u = mat_r[2][0] * pt_0.x
         + mat_r[2][1] * pt_0.y
         + mat_r[2][2] * pt_0.z;
  } catch (...) {
    throw;
  }

  return pt;
}

/*
 * @brief      関数 N
 *
 * @param[in]  X (double)
 * @return     計算結果 (double)
 */
double n(double x) {
  double res;

  try {
    res = kA / sqrt(1.0 - kE2 * pow(sin(x * kPi180), 2));
  } catch (...) {
    throw;
  }

  return res;
}

/*
 * @brief      BLH -> ECEF
 *
 * @param[in]  BLH  座標 (CoordB)
 * @return     ECEF 座標 (CoordX)
 */
CoordX blh2ecef(CoordB c_src) {
  CoordX ecef;

  try {
    ecef.x = (n(c_src.b) + c_src.h)
            * cos(c_src.b * kPi180)
            * cos(c_src.l * kPi180);
    ecef.y = (n(c_src.b) + c_src.h)
            * cos(c_src.b * kPi180)
            * sin(c_src.l * kPi180);
    ecef.z = (n(c_src.b) * (1.0 - kE2) + c_src.h)
            * sin(c_src.b * kPi180);
  } catch (...) {
    throw;
  }

  return ecef;
}

/*
 * @brief      BLH -> ENU
 *
 * @param[in]  BLH 座標(基準) (CoordB)
 * @param[in]  BLH 座標(対象) (CoordB)
 * @return     ENU 座標 (CoordE)
 */
CoordE blh2enu(CoordB c_0, CoordB c_1) {
  CoordX ecef_x_0;
  CoordX ecef_x_1;
  CoordX ecef_x;
  std::vector<std::vector<double>> mat_0(3, std::vector<double>(3, 0.0));
  std::vector<std::vector<double>> mat_1(3, std::vector<double>(3, 0.0));
  std::vector<std::vector<double>> mat_2(3, std::vector<double>(3, 0.0));
  std::vector<std::vector<double>> mat;
  CoordE enu;

  try {
    // BLH -> ECEF
    ecef_x_0 = blh2ecef(c_0);
    ecef_x_1 = blh2ecef(c_1);
    ecef_x = {ecef_x_1.x - ecef_x_0.x,
              ecef_x_1.y - ecef_x_0.y,
              ecef_x_1.z - ecef_x_0.z};
    // 回転行列生成
    mat_0 = mat_z(90.0);
    mat_1 = mat_y(90.0 - c_0.b);
    mat_2 = mat_z(c_0.l);
    mat = mul_mat(mul_mat(mat_0, mat_1), mat_2);
    enu = rotate(mat, ecef_x);
  } catch (...) {
    throw;
  }

  return enu;
}

}  // namespace blh2enu

int main(int argc, char* argv[]) {
  namespace ns = blh2enu;
  ns::CoordB blh_0;  // 変換前の座標(基準)
  ns::CoordB blh_1;  // 変換前の座標(対象)
  ns::CoordE enu;    // 変換後の座標
  double az;         // 方位角
  double el;         // 仰角
  double dst;        // 距離

  try {
    // コマンドライン引数の個数チェック
    if (argc < 7) {
      std::cout << "Usage: ./blh2enu B_0 L_0 H_0 B_1 L_1 H_1" << std::endl;
      return EXIT_SUCCESS;
    }

    // 基準、対象の BLH(WGS84) 座標取得
    blh_0.b = std::stod(argv[1]);
    blh_0.l = std::stod(argv[2]);
    blh_0.h = std::stod(argv[3]);
    blh_1.b = std::stod(argv[4]);
    blh_1.l = std::stod(argv[5]);
    blh_1.h = std::stod(argv[6]);

    // BLH -> ENU
    enu = ns::blh2enu(blh_0, blh_1);

    // 方位角、仰角、距離
    az = atan2(enu.e, enu.n) / ns::kPi180;
    if (az < 0.0) { az += 360.0; }
    el = atan2(enu.u, sqrt(enu.e * enu.e + enu.n * enu.n)) / ns::kPi180;
    dst = 0;
    dst += enu.e * enu.e;
    dst += enu.n * enu.n;
    dst += enu.u * enu.u;
    dst = sqrt(dst);

    // 結果出力
    std::cout << std::fixed << std::setprecision(8);
    std::cout << "BLH_0: LATITUDE(BETA) = "
              << std::setw(12) << blh_0.b << " °" << std::endl;
    std::cout << "    LONGITUDE(LAMBDA) = "
              << std::setw(12) << blh_0.l << " °" << std::endl;
    std::cout << std::fixed << std::setprecision(3);
    std::cout << "               HEIGHT = "
              << std::setw(7)  << blh_0.h << " m" << std::endl;
    std::cout << std::fixed << std::setprecision(8);
    std::cout << "BLH:   LATITUDE(BETA) = "
              << std::setw(12) << blh_1.b << " °" << std::endl;
    std::cout << "    LONGITUDE(LAMBDA) = "
              << std::setw(12) << blh_1.l << " °" << std::endl;
    std::cout << std::fixed << std::setprecision(3);
    std::cout << "               HEIGHT = "
              << std::setw(7)  << blh_1.h << " m" << std::endl;
    std::cout << "-->" << std::endl;
    std::cout << "ENU: E = "
              << std::setw(12) << enu.e << " m" << std::endl;
    std::cout << "     N = "
              << std::setw(12) << enu.n << " m" << std::endl;
    std::cout << "     U = "
              << std::setw(12) << enu.u << " m" << std::endl;
    std::cout << "方位角 = "
              << std::setw(12) << az  << " °" << std::endl;
    std::cout << "  仰角 = "
              << std::setw(12) << el  << " °" << std::endl;
    std::cout << "  距離 = "
              << std::setw(12) << dst << " m" << std::endl;
  } catch (...) {
      std::cerr << "EXCEPTION!" << std::endl;
      return EXIT_FAILURE;
  }

  return EXIT_SUCCESS;
}

2. ソースコードのコンパイル

$ g++ -std=c++17 -Wall -O2 --pedantic-errors -o blh2enu blh2enu.cpp

3. 動作確認

コマンドライン引数に原点と対象の緯度、経度、楕円体高を指定して実行する。

以下は、参考文献内の実行例(仙台空の滑走路の長さ)と同じパラメータで実行。

$ ./blh2enu 38.13877338 140.89872429 44.512 38.14227288 140.93265738 45.664
BLH_0: LATITUDE(BETA) =  38.13877338 °
    LONGITUDE(LAMBDA) = 140.89872429 °
               HEIGHT =  44.512 m
BLH:   LATITUDE(BETA) =  38.14227288 °
    LONGITUDE(LAMBDA) = 140.93265738 °
               HEIGHT =  45.664 m
-->
ENU: E =     2974.681 m
     N =      388.988 m
     U =        0.447 m
方位角 =       82.550 °
  仰角 =        0.009 °
  距離 =     3000.006 m

以下は、島根県本庁舎から見た松江市本庁舎の位置の計算例。(但し、標高 0m と想定)

$ ./blh2enu 35.472222 133.050556 0 35.468056 133.048611 0
BLH_0: LATITUDE(BETA) =  35.47222200 °
    LONGITUDE(LAMBDA) = 133.05055600 °
               HEIGHT =   0.000 m
BLH:   LATITUDE(BETA) =  35.46805600 °
    LONGITUDE(LAMBDA) = 133.04861100 °
               HEIGHT =   0.000 m
-->
ENU: E =     -176.539 m
     N =     -462.213 m
     U =       -0.019 m
方位角 =      200.904 °
  仰角 =       -0.002 °
  距離 =      494.779 m
  • U の値や仰角が負の値になっているのは、地球が球体であるため。誤りではない。
  • 概ね、島根県本庁舎から西に 176.539m, 南に 462.213m の所に松江市本庁舎が位置しているということ。

4. 参考文献

※上記の参考文献のソースコード内、方位角の計算に誤りがあるので、注意。


人工衛星の正確な軌道計算等に利用できるでしょう。

以上。





 

Sponsored Link

 

Comments