经纬高LLA转地心地固ECEF坐标，公式，代码

经纬高转地心地固的目的

坐标系转换是gis或者slam系统常见操作。GNSS获取的一般是经纬高，经纬高在slam系统里无法应用，slam系统一般是xyz互相垂直的笛卡尔坐标系，所以需要把GNSS的经纬高转到直角坐标系地心地固ECEF或者高斯投影GKP。

划重点：地心地固假设地球是椭球，和地球刚性绑定，随地球旋转。

椭球的定义

地心地固假设地球是个椭球，并且赤道肯定是“均匀”的，所以对应于a=b>c的扁球面。

关于椭球的参数化，公式如下

对于地球ECEF的XYZ轴定义：

X轴：地心射向赤道上本初子午线和赤道角点；

Y轴：和XoZ平面垂直，指向东经90度（满足右手定则）；

Z轴：地心指向北极。

地球套用上边的公式，a=b，乘以cosβ是从椭球面投影到赤道平面，再乘以cosλ或者sinλ分别是投影到本初子午线后的沿x轴和沿y轴的坐标。（维基百科的这个公式前因后果和abc的说明没有，不能直接套用，仅供参考）
 

大地纬度与椭球法线

椭球的大地纬度，并不是从地面指向地球球心，而是从点p找法线，求和赤道的夹角，因为是椭球，所以法线不指向地心。

椭球各轴坐标公式

这个椭球的子午面x和y的求法需要椭球的知识，略过，其中x是子午面的x，y是指向地球北极的高度，也就是说，这里边的x=根号(x'**2+y'**2)，这里边的y=z'，带'的都是ECEF坐标系，具体换算入下图：

这个公式直接对应写出代码即可

代码

  Eigen::Vector3d LLA2ECEF(const Eigen::Vector3d &lla) {Eigen::Vector3d ecef;double lat = deg2rad(lla.x());double lon = deg2rad(lla.y());double alt = lla.z();double earth_r = pow(EARTH_MAJOR, 2)/ sqrt(pow(EARTH_MAJOR * cos(lat), 2) + pow(EARTH_MINOR * sin(lat), 2));ecef.x() = (earth_r + alt) * cos(lat) * cos(lon);ecef.y() = (earth_r + alt) * cos(lat) * sin(lon);ecef.z() = (pow(EARTH_MINOR / EARTH_MAJOR, 2) * earth_r + alt) * sin(lat);return ecef;}

代码2

inline Eigen::Vector3d lla2ecef(const Eigen::Vector3d &lla,const AngPosUnit ang = AngPosUnit::DEG) {Eigen::Vector3d ecef;double sin_lat = std::sin(convang(lla(0), ang, AngPosUnit::RAD));double cos_lat = std::cos(convang(lla(0), ang, AngPosUnit::RAD));double cos_lon = std::cos(convang(lla(1), ang, AngPosUnit::RAD));double sin_lon = std::sin(convang(lla(1), ang, AngPosUnit::RAD));double alt = lla(2);double Rn = SEMI_MAJOR_AXIS_LENGTH_M /std::sqrt(std::fabs(1.0 - (ECC2 * sin_lat * sin_lat)));ecef(0) = (Rn + alt) * cos_lat * cos_lon;ecef(1) = (Rn + alt) * cos_lat * sin_lon;ecef(2) = (Rn * (1.0 - ECC2) + alt) * sin_lat;return ecef;
}