經緯度轉三維直角座標

經緯高定義

本文經緯度和高度指GPGGA語句中的經緯高，定義在WGS-84橢球體上，座標系爲地心地固座標系，X軸從地心指向本初子午線與赤道線的交點，Z軸指向北極點，Y軸垂直於XOZ平面滿足右手座標系。如下圖所示，$\lambda$是經度，$\varphi$是緯度，這裏緯度的定義採用大地緯度，即過P點的橢球體表面法線與赤道面的夾角；$h$是P點到橢球體表面的距離。

WGS-84橢球體參數如下：

參數	數值
橢球的長半軸	6378137.0000000000（m）
橢球的扁率	1/298.257223563

推導過程

如上圖，P點的經緯高座標是 $(\lambda, \varphi, h)$，所求量爲矢量${r_p}$，並且${r_p=r_Q+hn}$。
由圖，可得


所以，
$R_N$爲卯酉圈半徑，關於卯酉圈半徑長度的推導詳見另一篇博客：ECEF和大地座標系的相互轉化
這裏給出卯酉圈半徑結果，

C++代碼：

void BLH2ECEF(double lat, double lon, double alt, 
              double &x, double &y, double &z)
{
  lat = lat * PI / 180;//To rad.
  lon = lon * PI / 180;
  double f = 1 / 298.257223563;//WGS84
  double a = 6378137.0;//WGS84
  double b = a * (1 - f);
  double e = sqrt(a * a - b * b) / a;
  double N = a / sqrt(1 - e * e * sin(lat) * sin(lat));//$R_N$
  x = (N + alt) * cos(lat) * cos(lon);//ECEF
  y = (N + alt) * cos(lat) * sin(lon);
  z = (N * (1 - (e * e)) + alt) * sin(lat);
}

參考：
[1] ECEF和大地座標系的相互轉化
[2] GNSS與慣性及多傳感器組合導航系統原理（第二版）[英]Paul D.Groves 國防工業出版社
[3] 慣性導航、衛星定位及其組合的基本原理 [加] Aboelmagd Noureldin等 國防工業出版社
[4] WGS84座標系下大地座標轉換爲空間直角座標