2、坐标系与旋转表示：地理坐标系、载体坐标系、欧拉角、旋转矩阵、四元数基础
好，咱们正式开始聊姿态估计。在做卡尔曼滤波之前，有个坎儿必须迈过去——坐标系和旋转表示。说白了，你得先知道“我在哪”和“我朝哪”，才能谈“我接下来会怎样”。
我刚开始做惯导那会儿，就吃过坐标系的亏。一个旋转矩阵写反了，整个导航结果直接飞到了太平洋对面。嗯，从那以后，我对这部分内容就格外小心。
2.1 地理坐标系：我们到底在说哪个“北”？
地理坐标系，也叫导航坐标系。它是个固定在地球上的参考系。我们通常用 n 系 来表示。
- X轴：指向东（East）
- Y轴：指向北（North）
- Z轴：指向天（Up），也就是垂直于地面向上
这就是经典的 ENU（东-北-天） 坐标系。你想想看，飞机平飞的时候，它的高度变化就是沿着Z轴在动。
2.2 载体坐标系：传感器自己的“小世界”
载体坐标系，也叫 b 系。它固定在运动物体上，比如你的手机、无人机或者汽车。
- X轴：指向载体的前方（Forward）
- Y轴：指向载体的右侧（Right）
- Z轴：指向载体的下方（Down）
这就是 FRD（前-右-下） 坐标系。注意，这里的Z轴是朝下的，和地理坐标系的天向正好相反。为什么会这样？因为航空航天的老传统，加速度计感受到的重力是向下的，所以Z轴朝下更符合物理直觉。
2.3 欧拉角：俯仰、横滚、偏航
欧拉角是描述“载体坐标系”相对于“地理坐标系”旋转的最直观方式。说白了，就是三个角度：
| 角度 | 英文 | 绕哪个轴转 | 范围 |
|---|---|---|---|
| 俯仰角 | Pitch (θ) | 绕Y轴（右） | -90° ~ +90° |
| 横滚角 | Roll (φ) | 绕X轴（前） | -180° ~ +180° |
| 偏航角 | Yaw (ψ) | 绕Z轴（下） | 0° ~ 360° |
你想想看，飞机抬头就是俯仰角为正，侧身转弯就是横滚角，机头指向就是偏航角。非常直观。
但是，欧拉角有个著名的毛病——万向锁。当俯仰角接近 ±90° 时，横滚和偏航的旋转轴会重合，导致丢失一个自由度。我记得在调试一个四轴飞行器时，飞控突然抽风，就是因为俯仰角太大触发了万向锁。从那以后，我只要做全姿态估计，就坚决不用欧拉角做内部运算。
2.4 旋转矩阵：数学上的“硬核”表达
旋转矩阵 C 是一个 3x3 的矩阵。它能把一个向量从载体坐标系变换到地理坐标系，或者反过来。
比如，载体坐标系下的一个向量 vb，要转换到地理坐标系 vn，公式就是：
v^n = C_b^n * v^b其中 Cbn 就是从b系到n系的旋转矩阵。它由三个基本旋转矩阵相乘得到：
C_b^n = C_z(ψ) * C_y(θ) * C_x(φ)这里 C_z(ψ) 是绕Z轴转偏航角，C_y(θ) 是绕Y轴转俯仰角，C_x(φ) 是绕X轴转横滚角。注意顺序，先转横滚，再转俯仰，最后转偏航。
2.5 四元数基础：为什么它比欧拉角更香？
四元数，听起来很玄乎，其实就是一个四维的复数扩展。形式是：
q = q0 + q1*i + q2*j + q3*k其中 q0 是实部，q1, q2, q3 是虚部。它满足 i² = j² = k² = i*j*k = -1。
为什么我们要用四元数？三个理由：
- 无万向锁：这是最大的优势。无论你怎么转，四元数都能平滑表示。
- 计算量小：旋转矩阵有9个元素，四元数只有4个。在嵌入式系统里，少算几个乘法，CPU就少发热一点。
- 易于插值：在两个姿态之间做平滑过渡，四元数的球面线性插值（SLERP）非常优雅。
四元数表示旋转的公式也很简洁。一个向量 v 绕单位轴 u 旋转 θ 角度，对应的四元数是：
q = [cos(θ/2), u*sin(θ/2)]然后旋转后的向量 v' 就是：
v' = q * v * q_conjugate这里 q_conjugate 是四元数的共轭，就是把虚部取反。嗯，这个乘法在代码里实现起来，其实就是几个乘法和加法，非常高效。
2.6 它们之间的转换：实战必备
在实际项目中，你经常需要在欧拉角、旋转矩阵和四元数之间来回切换。比如，传感器输出的是欧拉角，但卡尔曼滤波的状态量是四元数。
下面是一个简单的转换示例，从四元数到欧拉角：
// 已知四元数 q = [q0, q1, q2, q3]
// 计算俯仰角 pitch
float sin_pitch = 2.0f * (q0 * q2 - q3 * q1);
float pitch = asinf(sin_pitch); // 注意范围限制
// 计算横滚角 roll
float sin_roll_cos_pitch = 2.0f * (q0 * q1 + q2 * q3);
float cos_roll_cos_pitch = 1.0f - 2.0f * (q1 * q1 + q2 * q2);
float roll = atan2f(sin_roll_cos_pitch, cos_roll_cos_pitch);
// 计算偏航角 yaw
float sin_yaw_cos_pitch = 2.0f * (q0 * q3 + q1 * q2);
float cos_yaw_cos_pitch = 1.0f - 2.0f * (q2 * q2 + q3 * q3);
float yaw = atan2f(sin_yaw_cos_pitch, cos_yaw_cos_pitch);
这段代码我用了很多年。注意 asin 函数，当俯仰角接近 ±90° 时，输入值可能略超 [-1, 1] 范围，记得做钳位处理。
atan2 算偏航角，结果发现偏航角在 0° 和 360° 附近跳变。后来才意识到，四元数到欧拉角的转换，偏航角的结果范围是 [-π, π]，而我们需要 [0, 2π]。加个判断就能解决：if (yaw < 0) yaw += 2*M_PI;。
好了，这一章的内容就到这里。坐标系和旋转表示是姿态估计的基石。你想想看，如果连“北”都指错了，后面的卡尔曼滤波再漂亮也是白搭。下一章，我们会把这些旋转表示用到卡尔曼滤波的状态方程里，真正开始做姿态解算。