2. 惯性导航基础

IMU预积分与误差标定的所有推导，最终都要落到“坐标系、姿态、测量模型”这三块基石上。如果坐标系定义混乱、姿态表示选错、测量模型理解偏差，后续的预积分残差和标定参数都会失去物理意义。本节把惯性导航最核心的基础概念梳理成一条可落地的工程链路。

一、坐标系定义：惯性导航的“语言”

惯性导航中至少涉及四个坐标系，它们之间的变换关系决定了所有测量量的物理含义。

• 惯性坐标系（i系）：原点在地心，坐标轴指向恒星，不随地球自转。牛顿力学定律在此系下严格成立，是陀螺和加速度计测量的理论参考。
• 地球坐标系（e系）：原点在地心，随地球自转，z轴指向北极。用于描述载体相对地球的位置。
• 导航坐标系（n系）：通常取“东-北-天”（ENU）或“北-东-地”（NED）。它是解算姿态和速度的参考系，也是预积分中“导航状态”的载体。
• 载体坐标系（b系）：固连在IMU上，原点在IMU中心，轴指向与器件安装方向一致。陀螺和加速度计的输出都是b系下的量。

工程上最容易出错的是n系的选取。ENU和NED的z轴方向相反，导致重力向量符号、姿态角定义、预积分速度更新公式全部不同。团队协作时必须先统一n系约定，再写代码。

二、姿态表示：旋转的三种“写法”

姿态描述的是b系相对n系的旋转关系。常用表示方法有三种，各有适用场景。

表示方法	参数个数	优点	缺点	典型用途
旋转矩阵 R	9（约束后3）	无奇异，直接作用于向量	冗余，正交约束需维护	预积分状态传播
四元数 q	4（约束后3）	紧凑，无奇异，插值平滑	符号二义性，需归一化	姿态滤波、优化
欧拉角	3	直观，易读	万向锁，三角函数开销大	人机显示、调试

预积分中通常用旋转矩阵或四元数做状态传播，欧拉角只用于最终输出和可视化。四元数的符号二义性（q和-q表示同一旋转）在优化中会导致雅可比不连续，工程上常用“保持标量部分为正”或“与上一时刻点积为正”来统一符号。

三、比力与角速度测量模型

IMU的核心输出是角速度ω和比力f。陀螺测量的是b系相对i系的角速度在b系下的投影，加速度计测量的是比力，即“非引力外力产生的加速度”。

理想测量模型可写为：

ω_m = ω_true + b_g + n_g
f_m = R_bn^T (a_n - g_n) + b_a + n_a

其中b_g、b_a为陀螺和加速度计的零偏，n_g、n_a为白噪声，g_n为导航系下的重力向量。注意比力公式中的负号：加速度计测量的是“支撑力产生的加速度”，自由落体时输出为零。

四、IMU器件分类与输出特性

按精度和成本，IMU大致分为消费级、工业级、战术级和导航级。不同级别器件的零偏稳定性、噪声密度、带宽差异巨大，直接决定预积分和标定策略。

• 消费级MEMS：零偏稳定性约1~100°/h，噪声密度大，适合短时、低精度场景，如手机、无人机。
• 工业级MEMS：零偏稳定性约0.1~1°/h，用于机器人、自动驾驶，是预积分算法的主战场。
• 战术级：零偏稳定性约0.01~0.1°/h，光纤或MEMS，用于航空、制导。
• 导航级：零偏稳定性优于0.001°/h，激光陀螺为主，用于潜艇、导弹。

输出特性方面，需关注三个关键参数：

1. 零偏：分为常值零偏、零偏稳定性、零偏重复性。常值零偏可通过标定补偿，零偏稳定性决定预积分能维持多久。
2. 噪声密度：白噪声的功率谱密度，单位如°/√Hz、m/s²/√Hz。它决定了预积分协方差的增长速度。
3. 带宽与采样率：采样率过低会丢失高频运动信息，过高则增加计算负担。预积分通常要求200Hz以上。

五、从测量到预积分：一条完整的信号链

把上述概念串起来，IMU原始输出到预积分量的信号链如下：

这条链路上，任何一个环节定义不清，都会导致预积分结果与真实运动不符。例如，若把加速度计输出直接积分而不扣除重力，速度会以9.8m/s²的速率发散；若n系选错，重力向量符号相反，预积分残差会整体偏移。

六、复盘：三个最容易踩的坑

1. 坐标系混用：代码中同时出现ENU和NED，导致重力向量、姿态角、速度方向全部错乱。建议在代码中显式命名坐标系，如R_bn_enu、g_ned。
2. 四元数未归一化：长时间积分后四元数模长漂移，旋转矩阵不再正交。每次传播后必须归一化。
3. 忽略零偏时变：消费级IMU零偏随温度和时间变化，若只用出厂标定值，预积分误差会快速累积。工程上应在线估计零偏，或缩短预积分窗口。