精读笔记

Problem Setting

论文标题：Invariant Smoother for Legged Robot State Estimation With Dynamic Contact Event Information（IEEE Transactions on Robotics / 2024）。

这篇论文处理的是腿式机器人 proprioceptive-only state estimation：只依赖 IMU 和关节编码器，在假设接触足静止的前提下估计 base orientation、velocity、position 以及接触足位置。它不是在追求全局 SLAM，也不是解决长期无漂移定位，而是在控制闭环所需的局部状态估计里，提高动态接触场景下的鲁棒性。

真正困难点是接触假设的脆弱性。leg kinematics 给状态估计提供强约束，但这个约束的物理前提是“接触足在世界系不动”。一旦脚滑、踩到不稳定地面、接触检测提前/滞后，观测因子就会把优化拉向错误状态。更麻烦的是，非 invariant smoother 的观测 Jacobian 往往依赖当前状态；当状态已经被错误接触拉偏时，下一轮线性化会在错误点附近构造错误局部模型，优化过程容易变慢、陷入次优或局部发散。

以前方法大致卡在两端：InEKF 有 invariant error 的收敛优势，但滤波器历史太短，无法重新判断过去接触事件，也无法表达跨多时刻的 foot-stationarity；fixed-lag smoother 能保留历史，但如果残差不是 invariant/group-affine，relinearization 反而会暴露在状态相关 Jacobian 的问题下。本文的关键矛盾就是：腿式估计既需要历史来处理接触事件，又需要稳定的线性化结构来避免动态接触时 smoother 被错误约束拖垮。

Motivation

作者的核心观察是：腿式机器人 proprioceptive estimation 的两个已有优势没有被充分合并。group-affine / invariant filtering 证明了合适的 Lie group 状态和 invariant residual 可以让误差动力学近似 log-linear，主要 Jacobian 不依赖当前估计；smoothing framework 则能保留历史、反复优化、处理跨时间约束。但现有工作要么只拿到 invariant 的滤波好处，要么只拿到 smoother 的历史好处，很少同时围绕动态接触事件设计。

已有路线不够的地方在于，接触质量不是一个瞬时事实，而是依赖状态估计本身的后验判断。某一时刻根据粗估状态算出的 foot velocity 可能误判滑移；等后续状态进入窗口后，这个判断应当被更新。滤波器做不到，因为过去状态已被边缘化。普通 smoother 虽然做得到，但其状态相关 Jacobian 在动态接触下会削弱优化可靠性。

所以这篇论文的动机不是“把 InEKF 改成 smoother”这么简单，而是：用 invariant formulation 降低 smoother 在坏接触下的线性化脆弱性，再用 smoother window 重新评估接触事件并加入跨时间 foot consistency。这是一个围绕接触假设可信度的建模重组。

Core Idea

核心思想可以概括为：把腿式状态估计中的接触足位置纳入 Lie group 状态，使 leg kinematic observation 也能写成 invariant observation，而不是把足端位置放在普通向量空间里用非 invariant 残差处理。这样一来，最关键的观测因子 Jacobian 变成状态无关或弱状态相关，优化不再强依赖当前线性化点的质量。对于动态接触，这一点尤其重要，因为坏接触最容易制造错误线性化点。

第二个核心思想是把接触事件作为 fixed-lag smoother 内的可重估信息流，而不是一次性 gate。SR 用 estimated foot velocity 判断接触假设是否可信，并通过调大 covariance 而不是硬拒绝观测来软化错误接触；由于窗口内状态会被更新，过去的 foot velocity 和接触状态也会被重新计算。CL 则进一步引入一个 inductive bias：如果一段时间内足端运动足够小，那么同一只脚在该时间段两端的世界位置应相同。这相当于在 proprioceptive-only setting 里构造了一个局部 loop closure，但 loop 的对象不是环境特征，而是接触足世界位置。

和 prior 的本质区别在于，它不是单纯增加 smoothing window，也不是单纯套用 InEKF 的状态定义；它改变了接触约束进入优化问题的方式：从状态相关、局部、一次性判断，变成状态无关线性化、窗口级重估、跨时刻一致性约束。

Method

1. Invariant state and residual design：作者把 R、v、p、contact foot position d 一起组织进 SE_k(3)，bias 保留在向量空间。这样做的目的不是形式优雅，而是让 forward kinematics observation 写成 right-invariant form。关键变化是 observation Jacobian 由依赖当前 R、p、d 的形式，变成由固定 s 向量诱导的常数结构。对优化型估计器来说，这直接减少了 relinearization 对当前状态的依赖。

2. Propagation factor：IMU propagation 和接触足零速度假设一起形成相邻时刻的 dynamics factor。由于 bias 不属于 group-affine 部分，传播 Jacobian 不是完全状态无关，但主要 manifold 部分仍继承 invariant error 的 log-linear 结构。这里的重点不是离散积分公式，而是把 IMU propagation 的误差表达放在 invariant error 上，使 Hessian 结构更稳定。

3. Slip rejection as covariance inflation：传统 SR 是检测到滑移就拒绝 kinematic measurement。本文改成当 foot velocity 超阈值时增大接触 covariance。这个改动机制上更合理：动态接触不是观测必然无效，而是静止接触假设置信度下降。软权重比 hard rejection 更适合 smoother，因为它允许后续优化重新调整这段历史。

4. Re-evaluation within fixed lag：foot velocity 由当前估计状态、IMU、关节速度计算；当窗口内状态更新后，过去时刻的 foot velocity 也会变。因此 SR 状态在窗口内被重估。这个机制是 smoother 相比 InEKF 的真实信息增益来源之一，本质上是 memory reuse / test-time compute，而不是新的传感信息。

5. Contact loop：在稳定接触持续超过多个 timestep 时，加入 d_a = d_b 的跨时刻约束。它解决的是相邻 propagation 中 foot stationary constraint 被噪声逐步稀释的问题；核心变化是把连续稳定接触压缩成一个更长时域的一致性约束。它类似局部 loop closure，但只在接触足这个 latent landmark 上闭环。

Key Insight / Why It Works

最关键的有效性来自两个机制叠加，而不是某个单独模块。

第一，state-independent Jacobian 降低了动态接触下 nonlinear optimization 的脆弱性。腿式估计里的坏观测往往不是随机 outlier，而是会系统性地把状态推向错误位置。如果残差 Jacobian 依赖当前姿态/位置，错误状态会改变局部问题几何，导致下一轮 GN step 的方向也变差。Invariant residual 的价值在这里非常具体：它不是抽象地“更优雅”，而是让优化问题在不同 operating point 下保持更一致的局部形状。论文中 PIS/NIS 在滑移时迭代次数显著上升、姿态偏差更大，正好支持这个解释。

第二，smoother 的收益主要来自历史接触信息的重估和跨时刻约束，而不是传统意义上的多次 relinearization。因为 invariant formulation 本身已经削弱了 relinearization 依赖，smoother 相比 InEKF 的额外价值并不主要是“可以反复线性化非线性模型”，而是能访问窗口内状态，重新计算 foot velocity、更新 slip covariance、添加 contact loop。这是 memory reuse / test-time compute 的收益。

第三，CL 的本质是给稳定接触注入一个强 inductive bias：接触足是短时局部 landmark。这个 bias 在 proprioceptive-only 场景非常有用，因为系统缺少外部 landmark；但它的可靠性完全取决于接触稳定性判定。一旦误判，CL 会变成强错误 loop closure。论文中 NIS + CL 可能变差，说明 CL 本身不是无条件增益，只有和更稳定的 invariant optimization 搭配才可靠。

哪些是核心贡献？我认为最实质的是“把足端位置纳入群状态以获得 invariant observation Jacobian”以及“窗口内重估接触事件”。CL 是有用但更像一个工程上有效的接触一致性因子；SR covariance inflation 是合理改良，但思想上接近已有 gating/robust weighting。整体不是 scaling，也不是数据覆盖驱动，而是 better inductive bias + memory reuse。增益来源比较清楚，但 SR/CL 的阈值和 covariance tuning 仍有明显 engineering 成分。

Relation To Prior Work

这篇论文处在 InEKF / invariant observer 与 fixed-lag smoothing 的交叉谱系上。最接近的是 Hartley 等人的 legged InEKF、Chauchat 等人的 invariant smoother，以及 Bloesch / Kim 系列的 kinematic odometry + slip rejection。

相对 InEKF，真正不同点不是把滤波器换成优化器，而是利用 fixed-lag window 表达滤波器不能表达的信息：过去接触状态可重估、稳定接触可跨时刻闭环。InEKF 的 invariant error 已经解决了一部分收敛问题，但它不保留足够历史，因此不能利用 CL，也不能修正之前的 slip 判定。

相对普通 fixed-lag smoother / NIS，真正不同点是状态定义和 residual geometry。NIS 通常只把 orientation 放在 SO(3)，速度、位置、足端位置在向量空间，kinematic residual 的 Jacobian 随当前姿态和足端几何变化；本文通过把 foot position 纳入 SE_k(3)，让观测成为 invariant observation。这是实质创新，而不是 notation change。

相对已有 SR，这篇把硬拒绝改成 covariance inflation，并且引入窗口内重估。前者更像稳健估计中的权重调整，创新性有限；后者更重要，因为它利用 smoother 的信息流改变了接触事件处理方式。

CL 看似新，其实思想上接近局部 loop closure / landmark consistency，只是 landmark 是接触足位置。它的新意在于针对腿式 proprioceptive estimation 的结构化使用，而不是数学上新。

Dataset / Evaluation

评价比较扎实的一点是有真机在线闭环控制，而不是只离线跑日志。HOUND 在室内滑板、软垫、木板碎石、外力拉拽等接触恶劣条件下运行，说明 estimator 至少达到了可部署水平。200 Hz、单次迭代、无 backtracking 的在线设置也支持作者关于 invariant formulation 对实时优化友好的 claim。

离线比较覆盖 IS、PIS、NIS、InEKF，并且有针对性地拆了 group-affine property、window size、SR/CL 的影响。这些实验基本能验证论文的两个核心判断：动态接触下 invariant smoother 比非 invariant smoother 更稳；window 增大后，由于 SR 重估和 CL，速度/位置误差下降。

但 evaluation 也有边界。平台单一，主要是 KAIST HOUND；接触条件虽然恶劣，但仍是作者自采场景，缺少跨机器人、跨地形的大规模 benchmark。户外 160 m 实验没有 ground truth，只能说明轨迹形状合理，不能强验证长期精度。误差指标也刻意避开不可观的 global position / yaw ATE，这是合理的，但意味着不能把结果解读成全局定位能力。

此外，阈值和噪声参数是经验调的；SR/CL 对这些参数的敏感性没有充分系统化。实验支持核心机制，但还没有完全排除 tuned engineering 对结果的贡献。

Limitation

1. 静止接触仍是根本前提。论文处理的是这个前提被短暂破坏时如何降权和恢复，而不是从动力学上理解滑移、摩擦、地形变形。若长时间全足滑移或接触持续不可靠，系统会退化为 IMU integration，漂移不可避免。

2. 接触事件检测是估计依赖的。foot velocity 用估计的 base velocity、orientation、bias 和关节速度计算，因此 slip detection 与状态估计形成闭环。估计错会导致接触判断错，接触判断错又会影响估计。窗口重估缓解了这个问题，但没有从根本上消除自举偏差。

3. CL 是强假设因子。它在稳定接触下很有效，但误判时相当于错误 loop closure。论文中 NIS + CL 变差说明 CL 的收益依赖优化几何；如果部署到更复杂地形，例如可移动支撑面、持续变形地面、软接触足端，CL 可能会把问题恶化。

4. group-affine 并不完整。bias 仍在向量空间，propagation Jacobian 中仍有与状态相关的项；文中也承认离散传播模型是从连续形式近似离散化，严格离散群仿射推导未完成。因此“state-independent Jacobian”应理解为主要观测结构和大部分关键项，而不是整个问题完全状态无关。

5. 无外感知的结构性上限没有改变。global position 和 yaw 不可观，长期漂移无法靠本文方法解决。户外结果没有真值，不能证明长期全局一致性。

6. 增益归因仍有混合。IS 同时改变了状态定义、残差结构、SR 策略、CL 因子和窗口历史使用；虽然消融较充分，但很难完全量化每个 engineering choice 的独立贡献。部分性能提升可能来自更保守的 covariance tuning 和更适合数据的接触阈值。

Takeaway

• 1. 对腿式 proprioceptive estimation，状态定义不是实现细节，而是决定优化几何的核心设计。
• 把接触足作为群状态的一部分，可以把 kinematic constraint 从状态相关残差变成 invariant observation，这是比单纯加 smoother 更根本的改变。
• 2. Smoother 在 invariant 系统里的价值不主要是 relinearization，而是 history-conditioned measurement qualification：过去接触是否可信，可以随着窗口内状态更新而重新判断。
• 这一 insight 可迁移到其他接触丰富系统，例如手爪操作、轮足机器人、触觉 SLAM。

一句话总结

这篇论文把 legged InEKF 的 invariant error geometry 和 fixed-lag smoother 的历史接触重估结合起来，实质贡献是用更好的状态/残差几何和接触事件信息流重组，提高 proprioceptive-only 腿式状态估计在动态接触下的优化鲁棒性。