2. 坐标系与运动学基础：世界坐标系与机体坐标系、旋转矩阵与四元数、刚体运动学方程、离散化运动更新
各位同学，欢迎来到IMU预积分的第一章基础课。
说实话，做IMU融合这么多年，我见过太多人一上来就啃预积分公式，结果被坐标系绕晕了。我自己刚入行时也犯过这毛病——在仿真里跑得飞起，一上真机就炸。后来才发现，坐标系定义错了，旋转矩阵用反了。嗯，这节课咱们就把这些地基打牢。
2.1 世界坐标系与机体坐标系
先问个问题：你坐在飞机上，手机屏幕朝上，这时候手机测到的加速度是9.8还是0？
答案是0。因为IMU测量的是相对于自身坐标系的运动。但我们要的是飞机相对于地面的运动。这就引出了两个核心坐标系。
- 世界坐标系（World Frame）：通常用W表示。我习惯选东北天（ENU）或者北东地（NED）。说白了，这就是我们定义“绝对位置”的参考系。重力方向在这里是固定的。
- 机体坐标系（Body Frame）：用B表示。原点在IMU中心，x轴朝前，y轴朝左，z轴朝上（右手系）。IMU读到的原始数据，全是在这个系下的。
核心要点：IMU预积分要解决的，就是把机体坐标系下的测量值，转换到世界坐标系下做积分。你想想看，如果坐标系搞反了，后面所有公式都是白搭。
我在项目中遇到过一件事：有个同学把IMU的z轴方向定义反了，结果积分出来的高度一直在往下掉。查了两天，最后发现是坐标系符号问题。所以，拿到IMU第一件事，先确认坐标系定义。
2.2 旋转矩阵与四元数
怎么把机体坐标系下的向量转到世界坐标系？靠旋转。
2.2.1 旋转矩阵
旋转矩阵R是一个3x3的正交矩阵，满足R^T R = I，行列式为+1。它的物理意义很直观：
p_w = R * p_b其中p_w是世界系下的坐标，p_b是机体系下的坐标。
我个人习惯用旋转矩阵做可视化调试，因为矩阵的每一列就是机体坐标轴在世界系下的方向向量。一眼就能看出飞机朝哪。
小技巧：旋转矩阵的逆等于它的转置。这意味着从世界系转回机体系，直接转置就行，不用求逆。这在代码里能省不少计算量。
2.2.2 四元数
旋转矩阵有9个参数，但自由度只有3。这就带来了冗余和数值问题。四元数用4个参数表示旋转，没有万向锁问题，插值也平滑。
四元数q = [q_w, q_x, q_y, q_z]^T，满足模长为1。旋转操作写成：
p_w = q * p_b * q^{-1}这里用的是四元数乘法。注意，p_b要写成纯四元数形式[0, x, y, z]。
避坑指南：我曾经在代码里把四元数乘法的顺序搞反了。Hamilton惯例和JPL惯例的乘法顺序是相反的。VINS、ORB-SLAM3用的是Hamilton，但有些论文用JPL。一定要看清楚你用的开源库是哪种！
为什么预积分要用四元数？因为我们要对旋转做积分。四元数的微分方程比旋转矩阵简单，而且没有冗余参数，数值稳定性更好。
2.3 刚体运动学方程
现在，我们把IMU的测量值和状态量联系起来。
IMU测量的是角速度ω和加速度a。注意，加速度包含了重力分量。所以运动学方程是：
p_dot = v
v_dot = R * (a_m - b_a - n_a) + g
R_dot = R * [ω_m - b_g - n_g]×解释一下：
- p是位置，v是速度，R是旋转矩阵
- a_m和ω_m是IMU原始测量值
- b_a和b_g是加速度计和陀螺仪的偏置（bias）
- n_a和n_g是测量噪声
- [·]×表示向量的反对称矩阵，用于实现叉乘
- g是世界系下的重力向量，通常取[0,0,-9.8]或[0,0,9.8]取决于坐标系定义
注意：这里的R_dot = R * [ω]×，而不是[ω]× * R。顺序很重要。我见过有人写反，结果旋转方向全错了。
说白了，这三个方程就是IMU融合的“物理引擎”。你给它一个初始状态和IMU数据，它就能推算出下一时刻的状态。
2.4 离散化运动更新
IMU数据是离散采样的，比如200Hz。所以我们要把连续方程离散化。
常用的离散化方法有两种：
| 方法 | 精度 | 计算量 | 适用场景 |
|---|---|---|---|
| 欧拉法 | 一阶 | 低 | 低动态、快速原型 |
| 中值法 | 二阶 | 中 | 大多数VIO系统 |
| RK4 | 四阶 | 高 | 高精度需求 |
我个人建议，做预积分用中值法就够了。为什么？因为IMU频率高（200Hz以上），时间间隔短，中值法的误差已经很小。而且预积分本身还要估计偏置，没必要在离散化上过度追求精度。
中值法的更新公式如下：
// 角速度取平均
ω_mid = (ω_k + ω_{k+1}) / 2
// 旋转更新
R_{k+1} = R_k * Exp(ω_mid * Δt)
// 加速度取平均（注意要转到世界系）
a_mid = (a_k + a_{k+1}) / 2
a_world = R_k * a_mid // 这里用R_k还是R_{k+1}？我习惯用R_k，因为R_{k+1}还没算出来
// 位置和速度更新
v_{k+1} = v_k + (a_world + g) * Δt
p_{k+1} = p_k + v_k * Δt + 0.5 * (a_world + g) * Δt^2经验之谈：上面的加速度更新里，我用了R_k而不是R_{k+1}。严格来说应该用平均旋转，但实际效果差别不大。如果你追求极致精度，可以用R_k和R_{k+1}的平均，但要多算一次指数映射。嗯，看你的算力取舍。
这里有个细节：Exp(·)是李代数到李群的指数映射。对于so(3)到SO(3)，其实就是罗德里格斯公式。代码里通常用Eigen的AngleAxisd或者Sophus的SO3::exp()。
注意：离散化时，旋转更新和位置更新是耦合的。你不能先更新位置再用新位置去算旋转。正确的顺序是：先算旋转，再算速度，最后算位置。我曾经在代码里把顺序搞反了，结果轨迹飞了。
好了，这一章的内容就这些。坐标系、旋转表示、运动学方程、离散化更新——这四个东西是IMU预积分的基石。你想想看，预积分说白了就是在两个关键帧之间，用IMU数据做相对运动估计。如果连坐标系和运动学都搞不清楚，后面预积分公式推导就是空中楼阁。
下一章，我们会正式进入预积分的核心——为什么需要预积分？它解决了什么问题？到时候见。