姿态表示与转换：旋转矩阵、四元数运算、欧拉角与四元数互转、姿态更新方程
各位同学，咱们今天聊点硬核的。姿态表示，说白了就是怎么描述一个物体在空间里「转」到了什么位置。搞IMU融合定位，这关过不去，后面全是空中楼阁。
我个人习惯把姿态比作「坐标系之间的姻亲关系」。你想想看，IMU装在载体上，它测得是载体坐标系下的数据，但我们最终要的是导航坐标系下的位置。这中间的桥梁，就是姿态。
1. 旋转矩阵：最直观，但最占内存
旋转矩阵，也叫方向余弦矩阵。它就是一个3x3的矩阵，把载体坐标系下的向量，映射到导航坐标系下。
公式很简单：v_nav = R * v_body
R矩阵的每一列，其实就是载体坐标系的基向量在导航坐标系下的投影。我记得刚入行时，为了理解这个，画了整整一晚上的坐标系草图。
优点： 直观，没有奇点，组合旋转就是矩阵乘法。
缺点： 9个参数，冗余。每次更新都要做9次运算，而且必须保证正交性。时间长了，数值误差会让它「歪掉」，需要重新正交化。
核心要点： 旋转矩阵是姿态的「完全体」，但工程上很少直接用，因为太「重」了。
2. 四元数运算：轻量级选手
四元数，很多人一听就头大。其实没那么玄乎。它就是一个四维的超复数：q = w + xi + yj + zk，其中w是实部，x,y,z是虚部。
为什么用它？因为只用4个参数，没有奇点，而且运算效率高。
四元数的乘法，说白了就是旋转的叠加。我给大家一个口诀：「实部相乘减点积，虚部交叉加叉积」。嗯，有点绕，但代码里就那么几行。
// 四元数乘法（C风格）
void quat_multiply(float q1[4], float q2[4], float q_out[4]) {
q_out[0] = q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2] - q1[3]*q2[3];
q_out[1] = q1[0]*q2[1] + q1[1]*q2[0] + q1[2]*q2[3] - q1[3]*q2[2];
q_out[2] = q1[0]*q2[2] - q1[1]*q2[3] + q1[2]*q2[0] + q1[3]*q2[1];
q_out[3] = q1[0]*q2[3] + q1[1]*q2[2] - q1[2]*q2[1] + q1[3]*q2[0];
}
注意，四元数乘法不满足交换律。顺序搞反了，旋转方向就反了。我在项目中吃过这个亏，调试了整整两天才发现是乘法顺序写反了。
小技巧： 四元数一定要归一化！每次更新后，记得做 q = q / norm(q)。否则模长会漂移，姿态就歪了。
3. 欧拉角与四元数互转
欧拉角（横滚、俯仰、航向）是给人看的。四元数是给机器算的。所以互转是家常便饭。
从四元数转欧拉角，公式如下（按ZYX顺序）：
roll = atan2(2*(w*x + y*z), 1 - 2*(x*x + y*y))
pitch = asin(2*(w*y - z*x))
yaw = atan2(2*(w*z + x*y), 1 - 2*(y*y + z*z))
反过来，从欧拉角转四元数：
w = cos(roll/2)*cos(pitch/2)*cos(yaw/2) + sin(roll/2)*sin(pitch/2)*sin(yaw/2)
x = sin(roll/2)*cos(pitch/2)*cos(yaw/2) - cos(roll/2)*sin(pitch/2)*sin(yaw/2)
y = cos(roll/2)*sin(pitch/2)*cos(yaw/2) + sin(roll/2)*cos(pitch/2)*sin(yaw/2)
z = cos(roll/2)*cos(pitch/2)*sin(yaw/2) - sin(roll/2)*sin(pitch/2)*cos(yaw/2)
警告： 欧拉角有「万向锁」问题。当俯仰角接近±90°时，横滚和航向会耦合，丢失一个自由度。我曾经在无人机飞控里遇到过，飞机倒飞时姿态解算直接崩了。所以内部运算永远用四元数，只在显示时转成欧拉角。
4. 姿态更新方程（微分方程）
这是核心中的核心。IMU输出的是角速度，我们要用它来更新姿态。说白了，就是解一个微分方程。
四元数的微分方程长这样：
dq/dt = 0.5 * q ⊗ ω其中ω是角速度向量（以四元数形式表示，实部为0），⊗表示四元数乘法。
离散化之后，常用的是一阶龙格-库塔法：
q_new = q_old + dq/dt * dt但更精确的是用角增量法。假设在dt时间内，角速度变化不大，我们可以直接计算旋转矢量：
Δθ = ω * dt
q_new = q_old ⊗ [cos(|Δθ|/2), (Δθ/|Δθ|)*sin(|Δθ|/2)]
我个人习惯用第二种。为什么？因为它在高动态下更稳定。我在做车载导航时，车辆急转弯时角速度变化剧烈，用一阶RK法姿态会飘，换成角增量法就稳了。
避坑指南： 我曾经在低端IMU上踩过坑——它的角速度采样率只有100Hz，而车辆振动频率远高于此。直接用原始角速度积分，姿态发散得厉害。后来加了低通滤波和圆锥补偿，才算搞定。所以，采样率不够时，别硬上，先滤波。
知识体系总览
下面这张图，是我自己画的。它把本章的知识点串起来了。你仔细看，就能明白它们之间的关系。
嗯，这张图你看懂了吗？从上往下看，姿态表示有三种方式，各有优劣。底部是它们对应的运算，而最下面那条，就是姿态更新的核心方程。所有东西都指向它。
好了，这一章的内容就到这。记住，姿态是IMU融合定位的「地基」。地基不稳，楼盖得再高也得塌。