卫星信号模拟器里的四个坐标转换公式

一套卫星信号模拟器里，坐标转换看起来是"边角料"，但其实贯穿始终：算仰角要用它、算电离层穿刺点要用它、生成用户轨迹要用它、上位机显示经纬高也要用它。这篇把工程里真正在用的四个转换函数拆开讲清楚。

0. 四个函数各干什么

函数	方向	用在模拟器哪里
`xyz2llh`	ECEF 直角坐标 → 经纬高	上位机显示、反解用户位置
`LlhToXyz`	经纬高 → ECEF 直角坐标	场景文件里给经纬高时转成直角坐标
`xyz2enu`	ECEF → 东北天(ENU)	算仰角、电离层/对流层误差
`enu2xyz`	东北天 → ECEF	用户轨迹生成（ENU 速度/位移 → ECEF）

坐标系约定都是 WGS-84 ：长半轴 a=6378137.0 m，短半轴 b=6356752.3142 m。

1. ECEF → 经纬高：`xyz2llh`

这是四个里最"重"的一个------反解本身没有初等闭式，工程上用的是先解中间量再一步到位的方法（经典来自 Kaplan《Understanding GPS》）：

c 复制代码

// 输入 xyz[3]（米），输出 llh[3] = {纬度, 经度, 高度}，纬度/经度为弧度
void xyz2llh(const double xyz[3], double llh[3])
{
    const double a = 6378137.0, b = 6356752.3142;
    const double e2 = 1.0 - (b*b)/(a*a);        // 第一偏心率平方
    const double ep = sqrt((a*a - b*b)/(b*b));  // 第二偏心率 e' = e·a/b

    double x = xyz[0], y = xyz[1], z = xyz[2];
    double r  = sqrt(x*x + y*y);                // 到地轴的投影距离
    double r2 = r*r, z2 = z*z;

    double E2 = a*a - b*b;
    double F  = 54*b*b*z2;
    double G  = r2 + (1-e2)*z2 - e2*E2;

    double c  = (e2*e2*F*r2) / (G*G*G);
    double s  = cbrt(1 + c + sqrt(c*c + 2*c));      // (...) 的立方根
    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  = (b*b*z)/(a*V);

    llh[2] = U*(1 - b*b/(a*V));                       // 高度
    llh[0] = atan((z + ep*ep*zo)/r);                  // 纬度
    llh[1] = atan2(y, x);                             // 经度
}

要点：经度直接 atan2(y,x)；纬度和高度靠中间量 ro（地心到大地纬面法线交点的近似距离）一次性解出。这个写法避免了迭代，在接收机和模拟器里都很常用。

2. 经纬高 → ECEF：`LlhToXyz`

这个有标准闭式，工程里写法略绕，但等价于大家熟悉的 N·cosφ·cosλ 形式：

c 复制代码

void LlhToXyz(const double llh[3], double xyz[3])
{
    const double a = 6378137.0, b = 6356752.3142;
    const double e2   = 1.0 - (b*b)/(a*a);
    const double ome2 = 1.0 - e2;                 // 1 - e²

    double lat = llh[0], lon = llh[1], h = llh[2];
    double clat = cos(lat), slat = sin(lat);
    double clon = cos(lon), slon = sin(lon);

    double tlat2 = (slat*slat)/(clat*clat);       // tan²φ
    double d     = sqrt(e2)*slat;                 // e·sinφ

    xyz[0] = a*clon/sqrt(1 + ome2*tlat2) + h*clon*clat;
    xyz[1] = a*slon/sqrt(1 + ome2*tlat2) + h*slon*clat;
    xyz[2] = a*ome2*slat/sqrt(1 - d*d) + h*slat;
}

等价性说明（这也是个不错的自查题）：

arduino 复制代码

1 + ome2·tan²φ = (cos²φ + (1−e²)sin²φ)/cos²φ = (1 − e²sin²φ)/cos²φ

所以 a/sqrt(1 + ome2·tan²φ) = a·cosφ/sqrt(1−e²sin²φ) = N·cosφ，其中 N 是卯酉圈曲率半径。代入就是教科书形式 x = (N+h)cosφcosλ，z = (N(1−e²)+h)sinφ。工程写法只是把 N 展开成了 tan²φ 和 d² 的形式，数值上更稳。

3. ECEF → 东北天：`xyz2enu`

以用户位置为原点建 ENU 坐标系，核心是一个旋转矩阵（来自 Leick《GPS Satellite Surveying》）：

c 复制代码

// 输入：xyz=目标点(卫星)ECEF，orgxyz=原点(用户)ECEF；输出 enu[3]={东,北,天}
void xyz2enu(const double xyz[3], const double orgxyz[3], double enu[3])
{
    double dx = xyz[0]-orgxyz[0], dy = xyz[1]-orgxyz[1], dz = xyz[2]-orgxyz[2];

    double llh[3];
    xyz2llh(orgxyz, llh);                 // 先反解用户经纬度
    double phi = llh[0], lam = llh[1];    // 纬度 φ、经度 λ

    enu[0] = -sin(lam)*dx + cos(lam)*dy;
    enu[1] = -sin(phi)*cos(lam)*dx - sin(phi)*sin(lam)*dy + cos(phi)*dz;
    enu[2] =  cos(phi)*cos(lam)*dx + cos(phi)*sin(lam)*dy + sin(phi)*dz;
}

有了 ENU，仰角就是 atan2(enu[2], sqrt(enu[0]²+enu[1]²))，方位角是 atan2(enu[0], enu[1])。电离层穿刺点、对流层映射函数也都从这里接着算。

4. 东北天 → ECEF：`enu2xyz`（这篇最有意思的一处）

ENU→ECEF 严格写法是上面旋转矩阵的转置：

c 复制代码

// 严格版（旋转矩阵的逆）
dx = -sin(lam)*e - sin(phi)*cos(lam)*n + cos(phi)*cos(lam)*u;
dy =  cos(lam)*e - sin(phi)*sin(lam)*n + cos(phi)*sin(lam)*u;
dz =                 cos(phi)*n         + sin(phi)*u;

但你工程里实际启用的 是一个小角度近似版：

c 复制代码

// 近似版（wcy-20210318，工程里在用）
void enu2xyz(const double enu[3], const double orgxyz[3], double xyz[3])
{
    double llh[3];
    xyz2llh(orgxyz, llh);
    double phi = llh[0], lam = llh[1], h = llh[2];

    // 1° ≈ 110 km：把东北位移近似成经纬度增量
    double d_lam = enu[0] / 110000.0 * (PI/180) / cos(phi);  // 东 → 经度差
    double d_phi = enu[1] / 110000.0 * (PI/180);              // 北 → 纬度差

    double llh2[3];
    llh2[0] = phi + d_phi;
    llh2[1] = lam + d_lam;
    llh2[2] = h   + enu[2];          // 天向直接加高度

    LlhToXyz(llh2, xyz);
}

为什么放着严格的旋转矩阵不用，用这个近似？ 注释里写得很实在："解决长时间运行高度不正确问题"。工程背景是------用户轨迹是长时间积分生成的，每一步的 ENU 增量都很小（米级），而严格的旋转矩阵版本在长时间累积、大数值坐标下反而会因浮点精度和模型细节（绕法线 vs 绕球面）引入高度漂移。小角度近似在"每步只走几米"这个前提下足够准（误差二阶小量），而且数值稳定。

但要记住它的适用边界 ：只适合小位移增量。如果要一次性换算一个大位移（比如几公里的 ENU 偏移），必须用旋转矩阵版，否则误差会明显。

5. 一张表收尾

函数	精确性	关键点
`xyz2llh`	闭式解，高精度	用中间量 ro 一步到位，不迭代
`LlhToXyz`	严格	写法绕，等价于 N 形式
`xyz2enu`	严格	旋转矩阵；仰角/方位角/误差模型都靠它
`enu2xyz`	小角度近似	仅用于轨迹小步进增量；大位移要用旋转矩阵版

卫星信号模拟器里的四个坐标转换公式