常用导航坐标系 及 转换关系 （理论+程序）

3）地理坐标系 （Geographic Coordinate System）：

是使用三维球面来定义地球表面位置，以实现通过 经纬度 对地球表面点位引用的坐标系。

一个地理坐标系包括 角度测量单位、本初子午线和参考椭球体三部分。在球面系统中，水平线是等

纬度线或纬线。垂直线是等经度线或经线。地理坐标系依据其所选用的本初子午线、参考椭球的不

同而略有区别。地理坐标系可以确定地球上任何一点的位置。首先将地球抽象成一个规则的逼近原

始自然地球表面的椭球体，称为参考椭球体，然后在参考椭球体上定义一系列的经线和纬线构成经

纬网，从而达到通过经纬度来描述地表点位的目的。需要说明的是经纬地理坐标系不是平面坐标系，

因为度不是标准的长度单位，不可用其直接量测面积长度。

地理坐标系 与 地心地固系（e 系）之间的转换关系如下：

程序如下：

Eigen::MatrixXd llh2ecef(Eigen::MatrixXd data) // transform the llh to ecef
{
  Eigen::MatrixXd ecef; // the ecef for output
  ecef.resize(3, 1);
  double a = 6378137.0;
  double b = 6356752.314;
  double n, Rx, Ry, Rz;
  double lon = (double)data(0) * 3.1415926 / 180.0; // lon to radis
  double lat = (double)data(1) * 3.1415926 / 180.0; // lat to radis
  double alt = (double)data(2); // altitude
  n = a * a / sqrt(a * a * cos(lat) * cos(lat) + b * b * sin(lat) * sin(lat));
  Rx = (n + alt) * cos(lat) * cos(lon);
  Ry = (n + alt) * cos(lat) * sin(lon);
  Rz = (b * b / (a * a) * n + alt) * sin(lat);
  ecef(0) = Rx; // return value in ecef
  ecef(1) = Ry; // return value in ecef
  ecef(2) = Rz; // return value in ecef
  return ecef;

  /**************for test purpose*************************
  Eigen::MatrixXd llh;
  llh.resize(3, 1);
  Eigen::MatrixXd ecef;
  ecef.resize(3, 1);
  llh(0) = 114.1772621294604;
  llh(1) = 22.29842880200087;
  llh(2) = 58;
  ecef = llh2ecef(llh);
  cout << "ecef ->: " << ecef << "\n";
  */
}



/*
function: ecef to llh
input: ecef (Matrix3d)
output: llh (Matrix3d)
*/
Eigen::MatrixXd ecef2llh(Eigen::MatrixXd data) // transform the ecef to llh
{
  Eigen::MatrixXd llh; // the ecef for output
  double pi = 3.1415926; // pi
  llh.resize(3, 1);
  double x = data(0); // obtain ecef 
  double y = data(1);
  double z = data(2);
  double x2 = pow(x, 2);
  double y2 = pow(y, 2);
  double z2 = pow(z, 2);

  double a = 6378137.0000; //earth radius in meters
  double b = 6356752.3142; // earth semiminor in meters
  double e = sqrt(1 - (b / a) * (b / a));
  double b2 = b*b;
  double e2 = e*e;
  double  ep = e*(a / b);
  double  r = sqrt(x2 + y2);
  double  r2 = r*r;
  double  E2 = a * a - b*b;
  double F = 54 * b2*z2;
  double G = r2 + (1 - e2)*z2 - e2*E2;
  double c = (e2*e2*F*r2) / (G*G*G);
  double s = (1 + c + sqrt(c*c + 2 * c));
  s = pow(s, 1 / 3);
  double P = F / (3 * ((s + 1 / s + 1)*(s + 1 / s + 1)) * G*G);
  double Q = sqrt(1 + 2 * e2*e2*P);
  double ro = -(P*e2*r) / (1 + Q) + sqrt((a*a / 2)*(1 + 1 / Q) - (P*(1 - e2)*z2) / (Q*(1 + Q)) - P*r2 / 2);
  double tmp = (r - e2*ro)*(r - e2*ro);
  double U = sqrt(tmp + z2);
  double V = sqrt(tmp + (1 - e2)*z2);
  double zo = (b2*z) / (a*V);

  double height = U*(1 - b2 / (a*V));

  double lat = atan((z + ep*ep*zo) / r);

  double temp = atan(y / x);
  double long_;
  if (x >= 0)
    long_ = temp;
  else if ((x < 0) && (y >= 0))
    long_ = pi + temp;
  else
    long_ = temp - pi;
  llh(0) = (long_)*(180 / pi);
  llh(1) = (lat)*(180 / pi);
  llh(2) = height;
  return llh;

  /**************for test purpose*************************
  Eigen::MatrixXd ecef;
  ecef.resize(3, 1);
  Eigen::MatrixXd llh;
  llh.resize(3, 1);
  ecef(0) = -2418080.9387265667;
  ecef(1) = 5386190.3905763263;
  ecef(2) = 2405041.9305451373;
  llh = ecef2llh(ecef);
  cout << "llh ->: " << llh << "\n";
  */
}

5）导航坐标系（n 系）：

6）载体坐标系（b 系）：

7）平台坐标系（p 系）、