精读笔记

Problem Setting

论文标题：On Lie Group IMU and Linear Velocity Preintegration for Autonomous Navigation Considering the Earth Rotation Compensation（IEEE Transactions on Robotics / 2025）。

这篇论文实际处理的是一个很具体但重要的导航问题：在图优化框架中，如何把高等级 IMU 和线速度传感器，例如 DVL、腿速、视觉里程计速度，预积分成一个概率一致的 motion factor，并且在长时间无外部感知约束时不被地球自转污染。

真正困难有两层。第一，线速度预积分并不是一个独立于 IMU 的量，因为把 body-frame velocity 积到 world-frame position 需要姿态，而姿态来自同一段 gyro measurement。因此 IMU preintegration 和 velocity preintegration 共享随机源。若把它们做成两个独立 factor，图模型表面上更简单，但隐含假设错了：factor independence 被破坏，相关性被丢掉或重复计入。第二，高等级 IMU 会真实测到地球自转。如果 residual 仍按 flat earth model 写，优化器只能通过 gyro bias 去吸收这个常量全局旋转，这会让 bias 估计不再是慢变随机游走，而变成和轨迹姿态耦合的伪补偿项。

这个任务的关键矛盾是：预积分想把高频传感器压缩成低频 factor，但压缩过程不能丢掉跨传感器相关性；同时，为了在线增量积累，又希望 raw preintegration 尽量局部、简单，不在每个 IMU sample 上做复杂全局惯性修正。论文的设计基本就是在这个矛盾之间找一个群论形式的分解。

Motivation

已有路线不够的地方不在于不能融合 DVL 或轮速，而在于融合方式常常是概率上不干净的。IMU factor 一条链、velocity factor 另一条链，看起来模块化，但两者都使用同一段 gyro 来做姿态传播；如果优化时把它们当作独立观测，信息矩阵结构就是错的。对高精度导航，这种错不是纯理论洁癖，而会直接影响 bias、姿态和位置误差的耦合。

作者的核心观察是：线速度测量之所以能改善 IMU preintegration，不只是因为它把位置从“双积分加速度”换成了“单积分速度”，更因为它给了 accelerometer bias 和 gyro bias 一个额外的动态一致性约束。但这个约束必须和 IMU 积分在同一个随机变量空间里表达，否则最有价值的相关性正好被建模方式切断。

另一个缺口是地球自转补偿和线速度联合预积分此前基本是分开的。已有 SE_2(3) IMU preintegration 已经说明 Lie group 上传播不确定性更自然，也有工作处理 REM；已有 leg/DVL velocity preintegration 则主要强调工程鲁棒性。本文动机就是把这两条线合并成一个统一的 SE_N(3) 预积分框架。

Core Idea

核心思想可以概括为：不要把 IMU 积分量和 LVS 积分量拆成多个 factor，而是把它们放进同一个扩展群元素里，让姿态、IMU 位置/速度、LVS 位置共享同一个 Lie group uncertainty propagation。对 IMU+LVS，作者使用 SE_3(3)：一个 rotation，加上三个 3D vector，分别表示由加速度得到的位置、速度、以及由线速度得到的位置。机器人真实状态仍然只有一个位置；双位置只存在于 measurement space，用来保留两条积分路径的信息差异和相关性。

这个建模改变了 prior work 的信息组织方式。传统做法是“IMU factor + velocity factor + 一个约束让姿态别重复”；本文则是“一个联合 factor 内部自然产生 cross-correlation”。本质区别是从 loosely assembled factors 变成 single tightly-coupled preintegrated measurement。它引入的 inductive bias 是：所有由同一段 rigid-body motion 和同一段 gyro 驱动的积分量，应被视为同一个随机过程的不同投影。

地球自转的处理也不是简单多减一个常量。作者把预积分模型写成 Γ · Φ(T, Δt) · Υ：Υ 只负责局部传感器读数累积，Γ 负责 world-frame global action，例如 gravity 和 Earth rotation，REM 中再通过 ψ 把旋转地球下的状态变换放进 residual。这个拆分让在线 preintegration 保持可增量计算，同时 residual 可以选择 FEM/REM 物理模型。

Method

1. SE_N(3) 作为统一预积分空间。它解决的是“多个 vector quantity 与同一 rotation 耦合”这一结构问题。相比 SO(3) × R^n 的组合流形，SE_N(3) 的 adjoint、Exp/Log、Jacobian 让协方差传播自动包含 rotation-vector 之间的耦合项。核心变化是：不确定性不再只是欧氏块对角传播，而是在群结构里传播。

2. 三种传感器配置只是同一个框架的实例。gyro+LVS 用 SE(3)，IMU-only 用 SE_2(3)，IMU+LVS 用 SE_3(3)。这不是模块罗列，而是说明 SE_N(3) 的 N 对应“需要随 rotation 一起传播的 3D 积分量个数”。真正新增的是 IMU+LVS 的 SE_3(3) 形式。

3. 双位置 measurement space。IMU 的 p_imu 和 LVS 的 p_lvs 在预积分量里同时存在，因为它们来自不同物理积分模型：constant global acceleration model 与 constant velocity model。状态空间仍是 SE_2(3)，通过 μ 映射到 SE_3(3)。这个设计避免了强行在预积分阶段把两种位置观测合并，也避免了丢掉二者差异中包含的 bias 信息。

4. Bias correction 与 covariance propagation。论文沿用预积分常见的一阶 bias 更新思想，但把 Jacobian 放在 SE_N(3) 上递推。它解决的是图优化中 bias 估计更新后不想重新积分所有高频测量的问题。机制上没有颠覆性新意，但对这个联合群空间是必要补全。

5. FEM/REM residual。FEM 只补偿 gravity；REM 补偿 gravity + Earth rotation。关键不在公式复杂度，而在 residual 层把 global action 从 local proprioceptive integration 中分离出来。实验中最关键的是旋转项 Γ_R 对 gyro Earth-rate 的补偿。

Key Insight / Why It Works

最核心的有效性来自两个机制：相关性保留和物理扰动显式建模。

第一，联合预积分有效，不只是因为 DVL 比加速度双积分更稳，而是因为 DVL 给了系统一个和姿态共享的、低漂移的位置积分通道。这个通道约束了 accelerometer bias 和 gyro bias 的组合漂移。如果 DVL 只是以低频 velocity prior 加进去，它只能局部修正速度；如果每个 DVL measurement 都被预积分进 motion factor，它会参与整个 keyframe interval 的相对运动约束。更重要的是，gyro noise 对 IMU-derived position 和 DVL-derived position 的影响是相关的，SE_3(3) factor 把这部分相关性留在 covariance 里。这是本文最实质的贡献。

第二，REM 有效是因为它阻止优化器把一个确定的 global rotation 错当成 sensor bias。高等级 IMU 中 Earth rate 的量级对一小时级别 dead reckoning 已经不可忽略；如果不建模，gyro bias 会被迫承担“随机器人姿态变化的地球自转投影”，这违反 Brownian bias 的慢变假设。REM 不是提高模型复杂度那么简单，而是把 bias 从一个错误的解释任务中解放出来，让 bias 重新表示传感器自身误差。

我认为论文中真正核心的是“single compact group joint factor + REM residual”。SE_N(3) 的一般化表述和解析 Jacobian 是重要工程/理论补全，但不是全部增益来源。实验中的位置提升很大程度也来自 DVL 全量使用，即从 sparse velocity prior 变成 dense preintegrated velocity constraint；这部分可能被表述成 joint Lie group 的收益，但文中没有充分做 ablation 来分离“更多使用 DVL 测量”和“正确建 cross-correlation”各自贡献。

Coriolis/centrifugal acceleration compensation 在该实验中基本没有体现收益，作者也承认没有观察到明显效果。因此 REM 的实际收益主要来自 gyro Earth-rate compensation，而不是完整旋转地球动力学。这里不要过度解读为所有 Earth-rotation terms 都重要。

这不是 scaling，不是 retrieval，也不是 data coverage 问题；它是 better inductive bias 和 representation alignment：把随机过程的结构、传感器共享信息、以及物理惯性场对齐到一个更合适的群表示里。

Relation To Prior Work

这篇属于 IMU preintegration on Lie groups 的技术谱系，直接继承 Forster 的 bias-corrected preintegration、Eckenhoff 的连续/离散模型讨论、Brossard 的 SE_2(3) 与 Earth rotation compensation。它不是从零提出新导航范式，而是把若干已经成熟的想法重新组合到一个更一致的概率建模框架里。

与 Forster/GTSAM 类方法的本质区别：不是在 SO(3) × R^3 × R^3 的组合流形上处理 IMU，而是用 SE_N(3) 统一表达 rotation 与多个 vector 的耦合；同时扩展到线速度积分。

与 Brossard SE_2(3) IMU preintegration 的区别：Brossard 主要解决 IMU-only 的群结构和 Earth rotation；本文把这个思想推广到 IMU+LVS，并引入 SE_3(3) 的双位置 measurement space。可以说本文是 Brossard 路线向多传感器 proprioceptive preintegration 的自然延伸。

与 Wisth/leg odometry 或 DVL velocity preintegration 的区别：prior work 往往使用两个 factor 或额外约束处理共享 orientation，本文强调单一 compact group factor，避免破坏 factor independence。这个点是实质创新，不只是符号重写。

看似新的部分里，bias 一阶修正、随机游走 bias、DVL lever-arm correction、iSAM2 图优化本身都不是新东西。真正新增的信息是：如何在 SE_N(3) 里同时容纳 IMU acceleration path 与 LVS velocity path，并给出 REM/FEM residual 与解析 Jacobian。

Dataset / Evaluation

评估是强真实世界导向的：AUV、真实 DVL、navigational grade FOG IMU、海上长时间 survey，并用 GNSS 作为离线 ground truth，同时与商业 iXblue UKF INS 对比。这比纯仿真或短时室内数据更能支撑“高等级 IMU 地球自转必须补偿”和“DVL 联合预积分提升 perception outage 鲁棒性”的 claim。

但评估覆盖面有限。任务基本是低速、平稳、水面、浅海、DVL 可稳定 bottom-lock 的 dead reckoning。它没有真正验证在 underwater SLAM、sonar scan matching、loop closure、视觉里程计失效、DVL dropout、高动态机动中的表现。论文声称框架可扩展到 VINS/LINS/legged robotics，这在理论上合理，但实验没有覆盖。

benchmark 是否支持核心 claim？支持 REM 的必要性，支持 DVL 预积分对一小时级 perception outage 有帮助，也支持 graph implementation 可达到商业 filter 同量级性能。但它没有充分支持“联合 compact factor 相比非联合 factor 显著更优”这一更细的 claim，因为缺少关键 ablation：例如两个 factor 但带 cross-covariance、两个 factor 不带 cross-covariance、DVL sparse prior vs dense preintegration 的系统对比。

此外，用商业 INS 作为对照有现实意义，但商业系统内部模型和调参不可见，比较更多是工程参考，不是严格机制验证。

Limitation

1. 增益归因不完全清楚。联合 factor、DVL 全量使用、较低 keyframe rate、REM compensation、bias random walk 调整同时变化，文中没有足够 ablation 分离各部分贡献。尤其“保留相关性”这一理论优势，在实验中没有被单独定量证明。

2. 方法依赖可靠 LVS。DVL 在平坦海底、稳定 bottom-track 时很强，但在悬停、复杂地形、弱回波、水体运动或高度变化时会出现系统误差。论文把 LVS bias 作为随机游走估计，但外参、声速、lever arm、scale 等误差的可观性没有充分讨论。所谓 DVL bias 可能吸收多种未建模误差，解释性有限。

3. REM 的收益场景有限但关键。对 consumer-grade MEMS，Earth rate 可能被噪声淹没；对短时 VIO，REM 复杂度未必值得。本文方法最适合 tactical/navigational grade IMU 和长时间 dead reckoning。泛化到低成本机器人时收益可能主要来自 LVS，而不是 Earth rotation。

4. 物理模型仍有简化。IMU preintegration 使用 CGAM，LVS 使用 constant velocity，二者在 SE_3(3) 中并存，但这也是把模型不一致交给优化器和协方差权重处理。高动态平台下，这种假设可能成为主要误差源。

5. Graph 优势没有在本文实验中真正释放。作者自己指出 dead reckoning 下 graph 不会显著超过 filter，因为没有闭环导致的长期线性化误差修正优势。本文更像是给未来 SLAM 插入一个更好的 proprioceptive factor，而不是证明 graph INS 本身优于 filter INS。

6. Scalability 上限在于状态和 factor 维度增加。SE_3(3) factor 加 bias 后维度不小；如果进一步加入多个 velocity-like sensors、外参在线标定、时间延迟标定，图规模和 Jacobian 复杂度会上升。框架 general，但不等于免费 scalable。

Takeaway

• 1. 对高等级 IMU，Earth rotation 不应被当成 bias 让优化器自己吸收；这会污染 bias 动态并导致航向不可控漂移。
• REM residual 是长时间预积分的基本卫生条件。
• 2. 多个 proprioceptive preintegration factor 共享 gyro 时，独立 factor 建模是有问题的。
• 更好的方向是构造联合 measurement space，让相关性在 covariance 中自然出现。

一句话总结

这篇论文是 SE_2(3) IMU 预积分向多传感器 proprioceptive 联合预积分的自然升级，核心贡献是在 SE_N(3) 中把 IMU、线速度积分和地球自转补偿组织成一个概率一致的紧耦合 graph factor。