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

Updated:


先日、 WGS84(World Geodetic System 1984) 測地系の緯度(Beta)/経度(Lambda)/楕円体高(Height)を ECEF(Earth Centered Earth Fixed; 地球中心・地球固定直交座標系)座標に変換する方法を C++ で実装しました。

今回は、逆に、 ECEF 座標を WGS84 の緯度(Beta)/経度(Lambda)/楕円体高(Height)に変換する方法を C++ で実装してみました。

過去には Python, Ruby, Fortran で実装しています。

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: ecef2blh.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
/***********************************************************
  ECEF -> BLH 変換
  : ECEF(Earth Centered Earth Fixed; 地球中心・地球固定直交座標系)座標を
    WGS84 の緯度(Latitude)/経度(Longitude)/楕円体高(Height)に変換する。

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

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

  ----------------------------------------------------------
  引数 : X Y Z
         (X, Y, Z: 元の ECEF 座標)
  ----------------------------------------------------------
  $ g++102 -std=c++17 -Wall -O2 --pedantic-errors -o ecef2blh ecef2blh.cpp
***********************************************************/
#include <cmath>
#include <cstdlib>   // for EXIT_XXXX
#include <iomanip>
#include <iostream>

namespace ecef2blh {

// 定数
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
};

/*
 * @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      ECEF -> BLH
 *
 * @param[in]  ECEF 座標 (CoordX)
 * @return     BLH  座標 (CoordB)
 */
CoordB ecef2blh(CoordX c_src) {
  double p;
  double theta;
  CoordB c_res;

  try {
    p = sqrt(c_src.x * c_src.x + c_src.y * c_src.y);
    theta = atan2(c_src.z * kA, p * kB) / kPi180;
    c_res.b = atan2(
      c_src.z + kEd2 * kB * pow(sin(theta * kPi180), 3),
      p       - kE2  * kA * pow(cos(theta * kPi180), 3)
    ) / kPi180;                                          // Beta(Latitude)
    c_res.l = atan2(c_src.y, c_src.x) / kPi180;          // Lambda(Longitude)
    c_res.h = (p / cos(c_res.b * kPi180)) - n(c_res.b);  // Height
  } catch (...) {
    throw;
  }

  return c_res;
}

}  // namespace ecef2blh

int main(int argc, char* argv[]) {
  namespace ns = ecef2blh;
  ns::CoordX c_src;  // 元の座標
  ns::CoordB c_res;  // 変換後の座標

  try {
    // コマンドライン引数の個数チェック
    if (argc < 4) {
      std::cout << "Usage: ./ecef2blh X Y Z" << std::endl;
      return EXIT_SUCCESS;
    }

    // 元の ECEF 座標取得
    c_src.x = std::stod(argv[1]);
    c_src.y = std::stod(argv[2]);
    c_src.z = std::stod(argv[3]);

    // ECEF -> BLH
    c_res = ns::ecef2blh(c_src);

    // 結果出力
    std::cout << std::fixed << std::setprecision(3);
    std::cout << "ECEF: X = "
              << std::setw(12) << c_src.x << " m" << std::endl;
    std::cout << "      Y = "
              << std::setw(12) << c_src.y << " m" << std::endl;
    std::cout << "      Z = "
              << std::setw(12) << c_src.z << " m" << std::endl;
    std::cout << "-->" << std::endl;
    std::cout << std::fixed << std::setprecision(8);
    std::cout << "BLH: LATITUDE(BETA) = "
              << std::setw(12) << c_res.b << " °" << std::endl;
    std::cout << "  LONGITUDE(LAMBDA) = "
              << std::setw(12) << c_res.l << " °" << std::endl;
    std::cout << std::fixed << std::setprecision(3);
    std::cout << "             HEIGHT = "
              << std::setw(7)  << c_res.h << " 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 ecef2blh ecef2blh.cpp

3. 動作確認

$ ./ecef2blh -3899086.094 3166914.545 3917336.601
ECEF: X = -3899086.094 m
      Y =  3166914.545 m
      Z =  3917336.601 m
-->
BLH: LATITUDE(BETA) =  38.13579617 °
  LONGITUDE(LAMBDA) = 140.91581617 °
             HEIGHT =  41.940 m

4. 参考文献


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

以上。





 

Sponsored Link

 

Comments