IEEE Transactions on Robotics

A Sliding Window Filter With GNSS-State Constraint for RTK-Visual-Inertial Navigation

作者：Xiaohong Huang, Cui Yang, Miaowen Wen · 机构：School of Electronic and Information Engineering, South China University of Technology, Guangzhou, China; Guangdong Provincial Key Laboratory of Short-Range Wireless Detection and Communication, Guangzhou, China

会议/期刊：IEEE Transactions on Robotics · 发布日期：2024 · 页码：1920-1937

传感器移动机器人视觉定位建图状态估计

精读笔记

Problem Setting

论文标题：A Sliding Window Filter With GNSS-State Constraint for RTK-Visual-Inertial Navigation（IEEE Transactions on Robotics / 2024）。

这篇论文实际处理的是 RTK-VIO 中一个很具体但关键的问题：GNSS raw measurements，尤其 carrier phase，对采样时刻、跨历元连续性和 ambiguity covariance 非常敏感；但视觉滑窗通常只保留视觉关键帧，GNSS 频率和视觉关键帧并不同步。如果把 GNSS 观测插值到视觉关键帧，或者把 GNSS 状态提前消掉，就会在计算上省事，但建模上牺牲了 GNSS 观测的时序完整性。

真正困难点有两个：一是如何保留 GNSS epoch 级状态而不让滑窗爆炸；二是如何让视觉-惯性的相对运动信息真正改善 ambiguity resolution，而不是只在 float solution 上看起来更平滑。以前方法卡在“为了实时性牺牲 GNSS 状态”与“为了 AR 需要多历元一致性”之间。本文的关键矛盾就是：RTK 要更多、更原始、更时刻准确的 GNSS 约束；VIO 滑窗为了实时性又倾向于只保留少量视觉关键帧。

Motivation

作者对已有路线的不满很明确：很多 tightly coupled GNSS-VIO 并没有真正把 GNSS epoch 当作一等状态来建模，而是通过插值、关键帧选择或预积分表达式把 GNSS 观测接到视觉关键帧上。这样做的问题不是简单的“精度下降”，而是会改变残差之间的相关结构。例如同一段 IMU measurements 同时用于 inertial residual 和 GNSS residual 的表达时，残差协方差本来不应为零，但已有做法经常近似忽略。

另一个关键缺口是 AR 分析。很多工作默认额外传感器提升定位精度就会提升 AR，但作者指出这个推理不成立。AR 需要拒绝错误整数 ambiguity；如果错误 ambiguity 下估计出的相对轨迹仍然与真实相对轨迹一致，那么再强的相对定位约束也未必能帮忙。本文的动机因此不是“加 VIO 让 RTK 更准”这种泛化表述，而是：需要一种建模方式，使 VIO/IMU 的相对几何约束能够在错误 ambiguity 假设下产生可观测的不一致性，并且这个不一致性能进入 ambiguity covariance 和 LAMBDA 搜索。

Core Idea

核心想法可以概括为：不要把 GNSS 测量迁就到视觉滑窗结构里，而是改造滑窗结构以容纳 GNSS epoch。所有 GNSS frames 都作为 keyframes 保留，状态包括天线位置、速度、姿态和 IMU bias；GNSS residual 在真实 GNSS 时间戳处约束状态，再通过 IMU factor 与视觉关键帧连接。这让 GNSS、IMU、视觉之间的信息交换发生在正确的时间拓扑上，而不是靠线性插值或丢弃非关键帧观测。

本质区别在于它把“GNSS 是稀疏全局观测”改成了“GNSS 是高频、多历元、与 ambiguity 共享状态的结构化约束”。引入的 inductive bias 是：跨历元相对运动应该由视觉-惯性解释，而 carrier phase ambiguity 的错误整数假设会在不稳定卫星跟踪时破坏这种相对一致性。这个 bias 对 AR 比单纯的 position prior 更有用，因为它约束的是错误整数假设下系统能否同时解释 GNSS carrier phase 和 VIO/IMU relative motion。

Method

1. GNSS preprocessing：解决的问题是 GNSS raw residual 近线性但包含 clock states 和 phase biases，若直接塞进滑窗会增加重复线性化与密集变量。作者利用卫星方向向量在局部状态变化下近似常量这一事实，提前线性化 pseudorange、Doppler、RB-SD carrier phase，并边缘化每个 epoch 的 receiver clock，生成 GNSS prior。核心变化是把 GNSS raw factor 转成轻量线性 prior，同时保留对 position、velocity、phase bias 的约束。

2. GNSS-state sliding window：解决的问题是传统视觉关键帧滑窗丢掉大量 GNSS epoch 或用插值损坏 carrier phase 建模。作者把所有 GNSS frames 作为 keyframes，视觉关键帧仍按视觉策略选择；边缘化时以视觉关键帧区间为单位移除旧视觉帧和其间 GNSS 帧。核心变化是滑窗状态拓扑从“视觉主导”变成“视觉-惯性-GNSS 多时间轴共存”。

3. Phase bias and AR covariance extraction：解决的问题是 LAMBDA 需要 ambiguity estimate 和 covariance，而全状态协方差求逆不可接受。作者通过预定义消元顺序把 phase biases 放到最后，使 Cholesky factorization 后尾部 Schur complement 直接给出 phase-bias covariance。核心变化是把优化求解和 AR 所需协方差计算合并，而不是额外做一次昂贵矩阵运算。

4. Parallel elimination：解决的问题是保留所有 GNSS states 会导致滑窗节点数明显增加。作者利用 GNSS frames 与相邻 IMU factor、phase bias 的稀疏连接，在固定消元树上并行消元独立分支。这里更偏 engineering，但它让“保留 GNSS 状态”这个建模选择可部署。

5. Observability / AR analysis：方法层面最有价值的不是公式本身，而是作者明确区分了 positioning improvement 和 AR improvement。理论部分说明 VIO/IMU 的相对约束只有在错误 ambiguity 会导致跨历元相对位置偏离真实相对位置时才提升 AR；这通常发生于卫星跟踪不稳定、权重/可见卫星/cycle slip 变化时。

Key Insight / Why It Works

这篇最重要的 insight 是：RTK-VIO 的收益不应被理解为“VIO 给 RTK 一个更准的位置先验”，而应理解为“VIO/IMU 给 ambiguity 搜索提供跨历元相对一致性检验”。在稳定卫星跟踪下，错误 ambiguity 造成的 position bias 可能在各 epoch 近似相同，因此相对位移仍然正确；这时相对约束对 AR 的帮助有限。只有当卫星集合、观测协方差或 cycle slip 发生变化时，错误 ambiguity 引入的 position error 不再是常量偏置，相对轨迹会被扭曲，VIO/IMU 才能有效拒绝错误整数。

因此本文真正有效的部分是 GNSS-state constraint + ambiguity covariance extraction 的组合。前者保证 GNSS epoch 信息不被视觉关键帧压缩掉；后者保证这些信息能够进入 AR，而不是只改善最终 pose graph。并行消元、GNSS 预处理属于必要的系统工程：没有它们方法跑不起来，但它们本身不是主要 scientific contribution。

这不是 scaling，也不是数据覆盖驱动的方法；更像是 better state representation / better factor graph topology。它通过改变状态保留策略，让原本被丢弃或错误相关性处理的 GNSS 信息重新进入估计。可以说它是在做 memory reuse：保留 GNSS epoch-level memory，使 carrier-phase ambiguity 在滑窗内看到足够多的时间结构。

一个需要警惕的点是，实验增益可能部分来自完整工程实现，包括高质量同步、全站仪校准、合理的 fix-and-hold 策略、GNSS 预处理和多线程求解，而不一定全部来自“GNSS-state constraint”。但作者的理论分析确实解释了为什么在 GNSS-challenging 环境中收益更明显，这比单纯报 fixed rate 更有说服力。

Relation To Prior Work

它最接近三条路线：RTKLIB/RTK-INS 的 GNSS/INS EKF，VINS-Mono/GVINS/GNSS-VIO 类滑窗优化，以及 MSCKF/tightly coupled RTK-VIO。和 RTKLIB/RTK-INS 相比，它不是在 EKF prediction 里加 IMU，而是把视觉-惯性相对约束放进多历元优化，并让其参与 ambiguity covariance。和 GNSS-VIO/GVINS 相比，它的关键不是“更 tight coupling”，而是 GNSS epoch 状态保留策略和 AR 闭环。

看似新的部分里，GNSS 预处理、Schur complement、LAMBDA、fix-and-hold、IMU preintegration 都是已有工具；真正新增的是这些工具的组织方式：保留 GNSS states，预定义消元让 phase biases 尾部化，同时复用 Cholesky 结果得到 AR covariance。这个组合使得“保留更多 GNSS 信息”不只是精度追求，而成为一个可实时运行的 AR-aware smoother。

它属于 factor-graph / sliding-window smoothing 向 GNSS raw measurement 领域更深耦合的一步。相比 loose coupling 或把 RTK solution 当 measurement 的方法，本文新增的信息是 carrier phase ambiguity 与 VIO/IMU relative motion 之间的联合约束，而不是 RTK 解算后的位姿观测。

Dataset / Evaluation

实验覆盖了真实世界低成本硬件、基站约 4 km baseline、GNSS-friendly 和 GNSS-challenged 两类区域，并用全站仪提供毫米级 ground truth。这个设置对验证“GNSS 不稳定时 AR fixed rate 更稳定”是匹配的，因为 R2 木下环境确实制造了 carrier-phase tracking 不稳定。作者还分别比较了有无 fix-and-hold、有无基站的情形，能支撑 RTK-based 和 SPP-based 两类 claim。

不过 evaluation 的覆盖仍是有限的。场景主要是地面平台、中低速、短距离、相对可控环境；没有系统覆盖城市峡谷、多路径极强、长时间遮挡、室内外切换、高速车辆、视觉低纹理或夜间场景。实验结果能证明该设计在作者设备和场景中优于若干 baseline，但还不足以证明它对所有 GNSS-challenging deployment 都 robust。

对核心 claim 来说，实验最有价值的是：稳定卫星环境下所有方法 fixed rate 都高，挑战环境下本文优势明显，这与作者 AR improvement analysis 一致。比较中 GNSS-VIO/loose coupling 不能改善 AR，因此定位提升有限，这也支持“核心不是后端平滑，而是 AR-aware coupling”。但 ablation 仍不够干净：例如单独去掉 GNSS-state retention、保留预处理和并行消元，或者单独比较插值 GNSS vs epoch-state GNSS 的 AR covariance 差异，文中未充分说明。

Limitation

第一，方法依赖高质量时间同步和外参。GNSS epoch 状态保留的优势建立在 measurement timestamp 准确的前提下；如果 GNSS PPS、相机、IMU 同步有系统偏差，保留更多 GNSS states 可能反而把错误时序更强地注入优化。

第二，AR 提升不是无条件成立。作者自己已经指出，稳定卫星跟踪时相对约束对 AR 改善有限；真正受益的是 tracking status 不稳定导致错误 ambiguity 产生非恒定相对误差的情况。因此它不是普遍提升 RTK 可解性的魔法，而是对特定退化模式有效。

第三，视觉-惯性相对约束是核心假设之一。若视觉退化、IMU bias 未充分激励、低动态、长时间静止或纯运动导致可观性下降，系统对 AR 的辅助会减弱。论文做了 observability analysis，但真实环境中这些退化常常叠加出现，文中没有充分展示极端组合情况。

第四，scalability 上限明显。保留所有 GNSS frames 与 phase biases 会增加状态规模，尤其多星座、多频、多基站或长滑窗时 dense bias block 会变大。并行消元缓解了计算，但本质上仍是用计算和内存换建模正确性。若扩展到 multi-frequency multi-constellation RTK/PPP-RTK，phase bias 管理会更复杂。

第五，GNSS 预处理近线性假设通常合理，但在初值很差、卫星几何差或 SPP bias 大时，线性化误差边界没有被充分量化。文中说 GNSS near-linear，因此粗初值也可接受；这个判断工程上常成立，但不是没有风险。

第六，fix-and-hold 策略既提升 fixed rate，也可能引入错误固定后的灾难性约束。作者用连续 ratio-test 缓解，但错误 fixed 的检测、回滚、cycle slip 后重建等鲁棒机制没有深入展开。增益中有多少来自 fix-and-hold 与系统先验传播，归因不完全清晰。

Takeaway

  • 1. 对 RTK-VIO 来说，关键不是简单 tight coupling，而是 ambiguity-aware coupling：视觉-惯性信息必须进入 ambiguity covariance 和 integer search，而不只是后验 pose smoothing。
  • 2. GNSS epoch 不应总是被视觉关键帧结构吞掉。
  • 只要任务涉及 carrier phase、Doppler 或高频 GNSS raw measurements，按真实采样时刻保留状态往往比插值到视觉帧更干净。
  • 3. AR 改善的本质条件是错误整数假设会破坏跨历元相对一致性；这个 insight 可迁移到其他多传感器整数估计问题，例如 LiDAR/INS 辅助 RTK、wheel odometry 辅助 GNSS、甚至多机器人基线 ambiguity 解算。

一句话总结

这篇论文在 RTK-VIO 方向中的位置，是把滑窗优化从“融合 GNSS 位姿约束”推进到“保留 GNSS epoch 状态并让视觉-惯性相对结构直接参与 ambiguity resolution”的一类 AR-aware smoothing 方法。