坐标系与姿态表示：地心地固坐标系、导航坐标系、欧拉角、四元数、旋转矩阵的转换关系
做定位系统漂移补偿，绕不开坐标系和姿态表示。说白了，你得先知道“我在哪”，才能谈“我偏了多少”。
我个人习惯把坐标系比作“参考框架”。你站在地球上，和站在月球上，看到的“东”和“北”完全不一样。所以，搞定位补偿，第一步就是把这些坐标系理清楚。
1. 地心地固坐标系（ECEF）
这个坐标系，我刚开始接触时觉得挺抽象。它把地球中心作为原点，Z轴指向北极，X轴指向本初子午线与赤道的交点，Y轴按右手定则确定。
嗯，说白了，它是一个随着地球自转一起转的坐标系。你站在北京，你的ECEF坐标是固定的，不会因为你走路而变。但如果你坐飞机飞到上海，ECEF坐标就变了。
为什么用它？
- GPS卫星直接输出的是ECEF坐标
- 全球统一，没有投影变形
- 适合做长距离、大范围的定位计算
避坑指南：我曾经在项目中直接用ECEF坐标做短距离的平面计算，结果发现距离误差很大。因为ECEF是三维球面坐标，短距离时曲率影响不可忽略。后来我改用局部切平面坐标系（ENU）才解决问题。
2. 导航坐标系（NED / ENU）
导航坐标系，说白了就是“你站在地面上，感觉到的方向”。
常用的有两种：
- NED（北-东-地）：X轴指北，Y轴指东，Z轴指向地心。航空、无人机常用。
- ENU（东-北-天）：X轴指东，Y轴指北，Z轴指天。机器人、自动驾驶常用。
我个人习惯用ENU，因为Z轴向上，和大多数传感器的安装方向一致。你想想看，IMU的加速度计，静止时Z轴输出是重力加速度，向上为正，多直观。
ECEF转ENU的步骤：
- 选定一个参考点（比如基站位置）
- 计算参考点的ECEF坐标
- 构建旋转矩阵，将ECEF坐标旋转到ENU
// 伪代码：ECEF 转 ENU
// 参考点经纬度：lat0, lon0, alt0
// 目标点ECEF坐标：x, y, z
// 1. 计算参考点的ECEF坐标
xe0, ye0, ze0 = geodetic_to_ecef(lat0, lon0, alt0)
// 2. 计算差值
dx = x - xe0
dy = y - ye0
dz = z - ze0
// 3. 构建旋转矩阵
// 这里用到了经纬度
sin_lat = sin(lat0)
cos_lat = cos(lat0)
sin_lon = sin(lon0)
cos_lon = cos(lon0)
// ENU 坐标
east = -sin_lon * dx + cos_lon * dy
north = -sin_lat * cos_lon * dx - sin_lat * sin_lon * dy + cos_lat * dz
up = cos_lat * cos_lon * dx + cos_lat * sin_lon * dy + sin_lat * dz我的经验：做漂移补偿时，我建议把IMU数据统一到ENU坐标系下处理。因为IMU的原始数据是载体坐标系，而漂移补偿需要知道“相对于地面的姿态变化”。统一到ENU后，计算会清晰很多。
3. 姿态表示：欧拉角、四元数、旋转矩阵
姿态表示，说白了就是“你的手机现在朝哪个方向”。
有三种主流表示方法，我挨个说。
3.1 欧拉角
欧拉角用三个角度表示姿态：横滚（Roll）、俯仰（Pitch）、偏航（Yaw）。
嗯，直观，好理解。你想想看，飞机转弯时，飞行员看的就是偏航角。
但是，欧拉角有个致命问题——万向锁。
当俯仰角接近±90°时，横滚和偏航会耦合，丢失一个自由度。我在做无人机飞控时遇到过这个问题，飞机垂直爬升时，姿态解算突然跳变，差点炸机。
警告：如果你做的是全姿态系统（比如无人机、机器人），不要只用欧拉角做内部姿态更新。万向锁会让你在特定角度下完全失控。
3.2 四元数
四元数，说白了就是一个复数在三维空间的推广。它用四个数表示姿态：q = [w, x, y, z]。
我刚开始学四元数时觉得很难理解，但用多了就发现它真香。
四元数的优点：
- 没有万向锁
- 插值平滑（做姿态平滑补偿时特别有用）
- 计算效率高（比旋转矩阵少乘法）
// 四元数乘法（组合两个旋转）
// q1 和 q2 是两个四元数
// 结果 q = q1 * q2
q.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z
q.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y
q.y = q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x
q.z = q1.w * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.w我的习惯：在IMU数据融合（比如卡尔曼滤波）中，我全部用四元数做状态量。只在最后输出给用户时，才转成欧拉角。这样既避免了万向锁，又方便用户理解。
3.3 旋转矩阵
旋转矩阵是一个3×3的正交矩阵，用于将一个向量从一个坐标系旋转到另一个坐标系。
比如，把载体坐标系的加速度计读数，旋转到导航坐标系：
// 旋转矩阵 R 将向量 v 从载体坐标系转到导航坐标系
v_nav = R * v_body旋转矩阵的优点是直观，可以连续旋转。但缺点是参数多（9个数），而且必须保持正交性，否则会引入误差。
4. 三者之间的转换关系
这三者可以互相转换，我画了一张图帮你理解：
这张图展示了三种姿态表示方法之间的转换路径。我个人建议，在代码实现中，以四元数作为“中间格式”。
为什么？因为四元数转欧拉角和旋转矩阵都有现成的公式，而且数值稳定性好。
5. 实战中的选择建议
| 场景 | 推荐表示方法 | 原因 |
|---|---|---|
| IMU姿态解算（内部） | 四元数 | 无万向锁，适合滤波 |
| 用户界面显示 | 欧拉角 | 直观，容易理解 |
| 坐标变换（连续旋转） | 旋转矩阵 | 方便链式乘法 |
| 姿态平滑插值 | 四元数 | Slerp插值平滑 |
| 漂移补偿（卡尔曼滤波） | 四元数 | 状态量少，线性化方便 |
我的经验：做漂移补偿时，我建议把姿态表示统一成四元数。因为漂移补偿的核心是“估计姿态误差”，而四元数的误差模型比欧拉角简单得多。我曾经在项目中用欧拉角做误差状态卡尔曼滤波，结果在俯仰角接近90°时滤波器直接发散。换成四元数后，问题迎刃而解。
6. 避坑总结
- 不要混用坐标系：我曾经见过有人把ECEF坐标直接当ENU用，结果定位误差几十米。一定要明确当前数据在哪个坐标系下。
- 注意四元数的归一化：四元数必须保持单位长度，否则旋转会变形。每次更新后记得归一化。
- 欧拉角的旋转顺序：不同的应用可能用不同的旋转顺序（ZYX、XYZ等），转换时一定要确认顺序。
- 旋转矩阵的正交性：数值计算会导致旋转矩阵失去正交性，定期做正交化处理。
嗯，坐标系和姿态表示就讲到这里。这些是定位系统漂移补偿的“地基”，地基打不牢，后面再好的算法也白搭。