2. 坐标系基础：地心地固坐标系(ECEF)、地理坐标系(LLA)、导航坐标系(n系)、载体坐标系(b系)及其转换
各位同学，咱们今天聊聊坐标系。说实话，搞组合导航这么多年，我见过太多人在坐标系转换上栽跟头。我自己刚入行那会儿，也曾在ECEF和LLA之间来回倒腾，结果算出来的位置差了十几公里——嗯，那次教训挺深刻的。
坐标系这东西，说白了就是给空间中的点找个"家"。不同的家，有不同的规矩。咱们组合导航里常用的有四个：ECEF、LLA、n系、b系。一个一个来。
2.1 地心地固坐标系（ECEF）
ECEF，全称Earth-Centered Earth-Fixed。原点在地球质心，Z轴指向北极，X轴指向本初子午线与赤道的交点，Y轴按右手定则确定。这个坐标系跟着地球一起转，所以叫"地固"。
为什么用它？因为GPS卫星的星历算出来的位置，默认就是ECEF坐标。你想想看，卫星绕着地球飞，用ECEF描述最自然不过了。
ECEF坐标表示： (X, Y, Z)，单位通常是米。范围大概在 -6378137 到 +6378137 之间。
我记得有一次做车载导航，客户给的参考轨迹是ECEF格式的，但我们的IMU输出是LLA。当时没注意单位换算，直接拿米和度去算误差，结果可想而知——嗯，从那以后我养成了习惯：拿到数据先看坐标系和单位。
2.2 地理坐标系（LLA）
LLA就是经纬高：纬度(Latitude)、经度(Longitude)、海拔(Altitude)。这个大家最熟悉，手机地图上显示的就是这个。
但要注意，LLA不是笛卡尔坐标系。经度线和纬度线在地球表面是弯曲的。所以你不能直接用LLA坐标做加减法——除非你只想算个大概。
| 参数 | 符号 | 范围 | 单位 |
|---|---|---|---|
| 纬度 | φ | -90° ~ +90° | 度 |
| 经度 | λ | -180° ~ +180° | 度 |
| 海拔 | h | 负值到正值 | 米 |
小技巧： 我习惯在代码里把经纬度统一转成弧度再计算，避免角度和弧度混用。曾经有个同事直接用度数算sin/cos，结果轨迹画出来像心电图——嗯，你懂的。
2.3 导航坐标系（n系）
n系，也叫导航坐标系。通常取"东北天"（ENU）或"北东地"（NED）。原点在载体所在位置，X轴指向东（或北），Y轴指向北（或东），Z轴指向天顶（或地心）。
为什么需要n系？因为IMU输出的加速度和角速度是在b系下的，但导航解算需要在n系下进行。说白了，你得把传感器数据"转"到导航坐标系里才能用。
我个人习惯用ENU。为什么？因为Z轴向上，海拔增加时Z值变大，符合直觉。NED的话，Z轴向下，海拔越高Z值越小——嗯，容易搞混。
2.4 载体坐标系（b系）
b系，固定在载体上。原点在载体质心，X轴指向载体前方（前进方向），Y轴指向右侧，Z轴指向下方（或上方）。这就是所谓的"右前上"或"右前下"。
IMU输出的原始数据，比如加速度计测到的比力、陀螺测到的角速度，都是在b系下的。你想想看，传感器装在车上，它测到的当然是车本身的运动，而不是相对于地球的运动。
注意： 不同厂家的IMU，b系定义可能不同。有的用"右前上"，有的用"前左上"。我曾经踩过这个坑：两个IMU装在同一台车上，输出数据直接做差，结果发现差异巨大——后来才发现坐标系定义不一样。所以拿到IMU第一件事：看手册，确认b系定义。
2.5 坐标系之间的转换
好了，四个坐标系都认识了。接下来就是怎么互相转换。我画了一张图，帮你理清关系：
2.6 关键转换公式
咱们挑几个最常用的转换说说。先说ECEF转LLA。这个公式我手写过不下百遍：
// ECEF (X, Y, Z) 转 LLA (lat, lon, alt)
// 使用WGS84椭球模型
double a = 6378137.0; // 长半轴
double f = 1.0 / 298.257223563; // 扁率
double e2 = 2*f - f*f; // 第一偏心率平方
double lon = atan2(Y, X);
double p = sqrt(X*X + Y*Y);
double lat = atan2(Z, p * (1 - e2));
double N;
// 迭代计算纬度（一般3次就够）
for(int i = 0; i < 5; i++) {
N = a / sqrt(1 - e2 * sin(lat) * sin(lat));
lat = atan2(Z + e2 * N * sin(lat), p);
}
double alt = p / cos(lat) - N;避坑指南： 我曾经直接用一次公式算纬度，结果在高纬度地区误差达到了几十米。后来老老实实加了迭代，精度就上去了。嗯，有些东西不能偷懒。
再说n系到b系的转换。这个靠姿态矩阵，也就是方向余弦矩阵（DCM）。如果你有欧拉角（横滚roll、俯仰pitch、航向yaw），可以这样构建：
// 欧拉角转方向余弦矩阵（ZYX顺序）
// roll: 横滚角, pitch: 俯仰角, yaw: 航向角
double cr = cos(roll);
double sr = sin(roll);
double cp = cos(pitch);
double sp = sin(pitch);
double cy = cos(yaw);
double sy = sin(yaw);
// C_n^b：从n系到b系的旋转矩阵
double C[3][3] = {
{cp*cy, cp*sy, -sp},
{sr*sp*cy - cr*sy, sr*sp*sy + cr*cy, sr*cp},
{cr*sp*cy + sr*sy, cr*sp*sy - sr*cy, cr*cp}
};有了这个矩阵，你就可以把n系下的向量转到b系下。反过来，转置一下就是b系到n系。
2.7 实际项目中的坐标系选择
说了这么多，到底该用哪个坐标系？我个人的经验是这样的：
- 卫星定位： 输入输出用ECEF或LLA，看接口要求
- 惯性导航解算： 在n系下做，因为重力方向明确
- 传感器数据： 在b系下处理，因为IMU输出就是b系
- 显示和交互： 用LLA，用户看得懂
你想想看，如果你在n系下做导航解算，重力加速度就是(0, 0, -g)，多清爽。要是在b系下做，重力方向随着载体姿态变化，那计算量就上去了。
核心要点： 坐标系转换是组合导航的基石。ECEF和LLA之间是几何关系，n系和b系之间是姿态关系。搞清楚了这两组关系，后面的松耦合和紧耦合就好理解了。
好了，坐标系就聊到这儿。记住一点：每次做转换之前，先确认输入输出是什么坐标系，单位是什么。这个习惯能帮你省下不少调试时间——嗯，我踩过的坑，你就别踩了。