IEEE Transactions on Robotics

$\sqrt{\mathbf {VINS}}$: Robust and Ultrafast Square-Root Filter-Based 3D Motion Tracking

作者：Yuxiang Peng, Chuchu Chen, Kejian Wu, Guoquan Huang · 机构：Department of Mechanical Engineering, University of Delaware, Newark, DE, USA; Department of Department of Mechanical and Aerospace Engineering, George Washington University, Washington, DC, USA; XREAL Inc., Beijing, China

会议/期刊：IEEE Transactions on Robotics · 发布日期：2025 · 页码：6570-6589

优化传感器视觉定位建图状态估计

精读笔记

Problem Setting

论文要解决的是资源受限平台上的 VINS 状态估计可用性：既要快，又要在 float32 / embedded 上长期不数值崩溃，还要能在很短时间内动态初始化或重初始化。这个问题的困难不在视觉-惯性模型本身，而在估计器的数值形态与 VINS 结构之间的错配。

传统 covariance EKF/MSCKF 计算直接，但 dense P 在低精度下可能失正定；information / optimization 方法利用稀疏性，但 Hessian / square-root information 可能病态，求逆或解线性系统时容易放大误差。Square-root covariance filter 理论上正好适合这个缝隙：用 U^T U=P 表示协方差，天然保持 PSD，条件数比 P 更好。但传统 SRF 更新会破坏三角结构，或者只能 sequential scalar update，面对 VINS 中一次上百/上千视觉残差时计算不划算。

因此关键矛盾是：VINS 需要 batch vectorized update 才快，而 SRF 需要维护三角结构才稳定；以前方法很难同时做到。

Motivation

作者的出发点很明确：SRF 的理论优势已经存在几十年，但没有在 VINS 中变成主流系统，说明瓶颈不在估计理论，而在结构利用不足。近期 P-QR SRF 已经说明 square-root covariance VINS 是可行的，但 QR 仍然有可优化空间，尤其在 measurement dimension 大于 state dimension 的视觉更新里。

第二个动机是初始化。现有 dynamic initialization 多数要么恢复 3D feature / SfM，再做 visual-inertial alignment；要么构造包含 feature/scale/velocity 的较大线性系统。问题是极短窗口下视差小、feature triangulation 近退化、而且 feature 数量一多计算会爆。作者观察到：初始化真正急需的不是完整 map，而是足够好的 velocity 和 gravity，使 filter 能启动；feature 可以晚一点通过 SRF refinement 接入。

所以这篇论文缺口定义得比较准：不是追求更高上限精度，而是让 filter-based VINS 在低精度硬件、短窗口初始化和大测量更新下仍然稳定且极快。

Core Idea

核心思想是把 SRF 的“数值稳定性”与 VINS 的“结构稀疏性”绑定起来，而不是把 SRF 当作 EKF 的机械替换。LLT-based update 通过 C = I + U H^T R^{-1} H U^T 把 measurement update 转成一个 SPD 矩阵的 Cholesky 分解，再用 F^{-T}U 更新 square-root covariance。由于 U 和 F^{-T} 都是三角结构，更新不再需要昂贵的重新三角化；由于 C 在实际 VINS 中条件数通常很小，Cholesky 的稳定性足够。

更本质地说，它改变的是计算组织方式：prior 不再通过 dense P 更新，也不通过 information matrix 解病态系统，而是始终在 square-root covariance 空间内做三角结构保持。状态排序、marginalization、feature initialization、outlier test 都围绕“尽量不破坏 U 的上三角结构”重新组织。

初始化部分的核心不是更复杂的 BA，而是反过来减少未知量：先不估 feature depth，不估全局 yaw/position，只用 bearing 几何给 translation direction，再用 IMU metric motion 约束解 velocity 和 gravity。这个 inductive bias 很强：短窗口下 feature depth 最不可靠，那就先从问题里移除它。

Method

1. LLT-based SRF update：它解决的是 canonical SRF / P-QR 在大批量视觉测量下的计算瓶颈。通过构造 C = I + U H^T R^{-1} H U^T 并 Cholesky 分解，update 等价于 EKF，但避免直接维护 P，也避免信息矩阵求逆。核心变化是把更新成本集中在一个条件数通常好的 SPD 因子化上，并保持 square-root covariance 的上三角结构。

2. 结构化状态排序：它解决的是滑窗 filter 中 cloning / marginalization 频繁破坏三角结构的问题。保留状态放前，频繁 marginalize 的 clones/features 放后，使删除变量后 U 尽量仍接近上三角。这个设计看似 implementation，但实际是 SRF 能否快的关键 structural prior。

3. 统一批量视觉更新：MSCKF features 通过 nullspace projection 只约束状态；已有 SLAM features 直接参与 update；新 SLAM features 用 QR 分解分出对已有状态的约束和 feature covariance initialization。它解决的是“既要用 feature 信息，又不让 feature 维度拖垮 update”的问题。

4. Feature-less initialization：它解决短窗口初始化中 feature triangulation 不稳且慢的问题。用多 feature epipolar plane normals 求 relative translation direction，再把 IMU preintegration 的 position relation 投影到该方向的正交子空间，消掉未知 scale，直接解 velocity + gravity。

5. SRF refinement 初始化：它解决线性初始化不够准、以及后续 filter 需要 covariance 的问题。相比 VI-BA 后再反演 Hessian，SRF refinement 直接维护 covariance，且能把初始化窗口中的 SLAM features 带入后续 VINS，减少启动后的信息断层。

Key Insight / Why It Works

最核心贡献是 LLT-SRF update 与 VINS measurement-heavy 结构的匹配。VINS 中 measurement 数通常远大于 state 数，且 measurement noise 不会小到让 C 极端病态；因此用 Cholesky 代替 QR 在理论 FLOPs 和实际 BLAS 层面都更划算。这里的收益不是 scaling/data，而是更好的 numerical representation + structure-aware computation。

第二个关键 insight 是：square-root covariance form 的优势只有在整个系统围绕三角结构设计时才会显现。单独把 EKF 的 P 换成 U 不会快；真正有效的是状态排序、marginalization 位置、feature initialization 的 lower/upper triangular 设计、batch update、以及 U H^T 复用。这些看起来像 engineering，但属于机制层面的 engineering：它们决定了复杂度常数和是否能避免全矩阵重分解。

初始化的有效性来自一个明确的 latent structure：短窗口内 feature depth/scale 是弱约束，但 translation direction 可以由 bearing epipolar geometry 聚合得到；IMU 则提供 metric acceleration integration。投影到 translation direction 的正交子空间后，scale 被消除，问题退化为低维 velocity/gravity 解。这比先三角化 feature 更稳，尤其低视差时。

需要判断的是：论文中“100 ms minimal initialization”的表现并不完全来自初始线性系统本身，SRF refinement、保留 SLAM features、以及后续 VIO 成功判据共同贡献很大。feature-less 初值是必要条件，但系统鲁棒性增益很可能来自整个 initialization-to-filter 信息连续性，而不是某个闭式解单独碾压 prior。

哪些可能只是辅助？在线标定、anchor feature covariance propagation、outlier rejection 复用等更多是系统完整性和效率优化，不是核心科学贡献。哪些可能主要是 engineering？轻量配置 sqrt-VINS(L) 的极高频率部分显然包含状态/feature 数减少带来的 scaling 效应，不能完全归因于 SRF update。

Relation To Prior Work

它最接近三条线：MSCKF/OpenVINS 式 covariance filter、P-QR square-root covariance VINS、以及 dynamic initialization / LiGT / SfM+VI alignment 方法。

相对 MSCKF/OpenVINS，本质不同是 representation：不直接维护 dense P，而维护 square-root covariance U，因此避免失正定并改善 float32 稳定性。但状态、视觉 measurement、MSCKF feature nullspace projection 等并不是全新范式，更多是 filter-based VINS 的重组。

相对 SRIF / square-root information / optimization 方法，它强调 covariance form 的数值温和性。信息形式的稀疏性适合优化，但 Hessian 或 square-root information 长期可能病态；这里用 innovation/covariance 侧的良好条件数来换稳定性。这是一个明确的技术谱系选择。

相对 P-QR SRF，LLT 是实质创新：它利用 VINS 中 C 条件数通常很好的事实，用更便宜的 Cholesky 替换 QR，并通过 permutation/triangular solve 保持结构。严格说这是 linear algebra 层面的改进，但对实时 VINS 很关键。

相对初始化 prior，feature-less 线性系统是比较实质的新增信息：不恢复 3D feature，不保留多余 scale 变量，直接解 minimal inertial state。与 LiGT 类方法共享“避免 feature position”的精神，但它进一步把问题压缩到 velocity/gravity，并与 SRF covariance recovery 无缝连接。

Dataset / Evaluation

评估覆盖较完整：有数值仿真验证条件数和 float32 稳定性，有 EuRoC/TUM-VI 这类标准 VI 数据，也有 AEA 这种 AR 眼镜日常场景，还有 Jetson Nano 上的 runtime。对于论文核心 claim——快、float32 稳、短窗口初始化可用——这些实验基本是对位的。

最有说服力的是三类证据：一是 SRIF float 在长轨迹上因条件数变差而性能下降，而 SRF float 与 double 接近；二是 LLT 与 P-QR 的 update time ratio 随 measurement 增多接近理论趋势；三是动态初始化不仅看初始化窗口误差，还看后续 VIO 是否成功，这比只报 initial gravity/scale 更严格。

但 evaluation 也有局限。首先，很多 runtime gain 同时混合了算法形式、状态配置、feature 数、batching 策略和实现优化，归因并不完全干净。其次，AEA 上 OpenVINS 的 dynamic initialization / tuning 是否达到其最佳状态文中未充分说明。第三，动态场景和 outlier-heavy 场景虽然在 AEA 中部分存在，但没有系统性 stress test，因此“dynamic environments”仍是 future work，而不是已解决问题。

Limitation

1. LLT 的稳定性依赖 C 条件数良好。作者给出经验解释：VINS 中 measurement uncertainty 通常不会远小于 state uncertainty，因此 C 接近 I。但这是经验前提，不是无条件保证。极端高置信视觉、错误噪声标定、强相关测量、或不良 outlier 进入 update 时，Cholesky 可能比 QR 更脆弱。

2. 初始化假设并不弱。文中默认 IMU biases reasonably accurate，camera-IMU calibration 和 time offset 已知。对于消费级设备冷启动、温漂、撞击后重启，这些假设可能不成立。bias 初值误差对 100 ms 初始化的影响没有被充分展开。

3. Feature-less 不等于 feature-free。初始线性解不三角化 feature，但 refinement 仍要 triangulate / update features，并且最终鲁棒启动依赖把 SLAM features 和 covariance 带入 VINS。论文宣传的 minimal 100 ms 更像“线性初值 + SRF refinement + 信息连续性”的系统能力，而不是单个 feature-less solver 的能力。

4. Scalability 上限仍在 dense covariance / square-root covariance。虽然结构利用很强，但 SRF covariance form 不是天然稀疏图优化；当状态维度因多传感器、大地图、长窗口显著增长时，dense U 的二次/三次项仍会回来。该方法非常适合小中型滑窗 VIO，不必然适合大规模 SLAM。

5. 部分速度增益属于 engineering / configuration。LLT update 是核心；但 sqrt-VINS(L) 的超高速度还来自较少 clones/features/MSCKF updates。若和其他系统在完全相同状态规模、前端、outlier policy、batching 方式下比较，增益可能缩小。

6. 对动态物体和视觉前端失败的鲁棒性没有本质解决。SRF 改善的是 estimator 数值稳定性，不是 data association。若 feature tracks 被动态物体主导，epipolar direction 和 Mahalanobis gating 都可能失效。

Takeaway

  • 1. Square-root covariance filter 在 VINS 中真正有价值，但前提是系统级结构设计，而不是简单替换 EKF covariance。
  • 未来做低算力 VIO/VINS，representation + state ordering + update algebra 应该一起设计。
  • 2. LLT over C = I + U H^T R^{-1} H U^T 是一个可迁移 insight：当 measurement-heavy、covariance-side innovation 条件数良好时，Cholesky 可能比 QR 更适合实时 square-root filtering。
  • 3. 初始化不一定要先恢复 3D structure。

一句话总结

《√VINS》是 filter-based VINS 向数值稳定、结构感知、嵌入式友好的 square-root covariance 系统演化的一步，真正贡献在于把 SRF 的理论优势通过 LLT 更新、状态组织和 feature-less 初始化变成可部署的实时系统能力。