精读笔记
Problem Setting
[Iterated Invariant EKF for 3D Landmark-Aided Inertial Navigation]（arXiv preprint / 2026）
这篇论文处理的是已知三维 landmark map 辅助下的惯性导航：MAV 依赖 IMU 高频传播，并周期性接收相对于已知地标的三维位置观测。表面看是标准 VIO / landmark-aided localization，但核心矛盾不是“怎么融合 IMU 和 landmark”，而是滤波器如何在存在不可观方向时不虚假变得自信。
传统 SO(3)-EKF / Multiplicative EKF 的问题在于 measurement Jacobian 依赖当前绝对位姿估计。这样一旦线性化点偏了，滤波器会把不该观测到的信息注入 yaw / global position 等不可观方向，产生 false observability。IEKF 通过不变误差修正了这个结构，但单次 update 仍可能因测量非线性和大初始误差而落在错误的局部线性近似上。本文实际解决的是这两个问题的交集：既要 observability-consistent，又要在 landmark measurement 的非线性几何约束下有更强的收敛能力。
Motivation
已有方法的缺口很清楚：非不变 EKF 可以迭代，但迭代的是错误几何；IEKF 几何正确，但 update 仍是一阶局部线性化。作者的核心观察是，landmark measurement 不是普通 residual，而是在状态空间上定义 observed submanifold；理想低噪声情况下，更新后的状态应该落在这些观测子流形上，多 landmark 情况下应该走向其交集。
因此问题不只是减少线性化误差，而是让 update 的几何行为和观测约束一致。IterIEKF 的动机就是补齐 IEKF 的 measurement-side compatibility：传播阶段用 invariant error 保持正确可观测性，更新阶段用迭代优化把估计推向观测流形，而不是一次性 Kalman correction 后停在某个近似切平面。
Core Idea
论文的核心思想是：把 landmark-aided inertial navigation 组织成一个 invariant filtering + local MAP optimization 的问题。右不变误差让误差动力学尽量摆脱绝对轨迹依赖，SE_2(3) 表示把姿态、速度、位置的耦合纳入群结构；地标观测则通过群作用写成 invariant innovation。这样 measurement Jacobian 的关键结构不再跟随当前估计位姿漂移，而是保留系统对称性对应的可观测结构。
在此基础上，IterIEKF 不满足于 IEKF 的单次线性更新，而是在 invariant residual 上做 Gauss-Newton 迭代。它引入的 inductive bias 是：后验均值应该位于 observed state manifold，协方差应该局限于其切空间。与普通 iterated EKF 的本质差异不是“多迭代几次”，而是迭代发生在满足 invariant error geometry 的坐标系里；与标准 IEKF 的差异也不是换了传播模型，而是把 measurement update 从一阶投影升级为几何一致的局部优化。
Method
关键机制可以压缩为四点。
1. 右不变误差建模：状态嵌入 SE_2(3)×R^6，用 Log(X Xbar^{-1}) 定义误差。它解决的是传统误差参数化导致的轨迹依赖 Jacobian；核心变化是把可观测性结构编码进误差坐标，而不是事后用 FEJ / constrained EKF 修补。
2. invariant landmark innovation：将相对地标观测写成 X^{-1}d + noise，并构造 z = Xbar y - d。这样 linearized measurement matrix 对 invariant filter 呈现与 landmark geometry 相关的形式，减少对当前绝对估计的依赖。它解决的是 measurement update 中 spurious information 的入口。
3. IterIEKF update：在 invariant residual 上迭代重线性化，使用 right Jacobian 修正扰动传播。它解决的是 IEKF 单次 update 在大误差、低频 landmark update、非线性观测下的局部近似不足；核心变化是引入 test-time compute，把滤波 update 变成局部 MAP 求解。
4. 与 SO(3)-EKF / Iter SO(3)-EKF 对照：作者保留了非不变单步和迭代版本作为 baseline，使问题归因更清楚：仅迭代不够，仅 invariant 也不是最强，二者组合才给出主要增益。
Key Insight / Why It Works
最关键的 insight 是：consistency 和 accuracy 在这里不是两个独立问题。传统 EKF 的精度下降并不只是线性化误差，而是错误坐标系把不可观方向伪装成可观信息；一旦 covariance 在错误方向被压缩，后续更新会越来越自信地沿错误轨迹收敛。IEKF 先修正这个信息流方向，使 filter 不再从绝对位姿线性化点中制造虚假观测。
IterIEKF 的额外收益来自 test-time optimization，但不是普通意义上的“多算几步更准”。它有效是因为迭代的 residual 与不变误差结构兼容：每次更新都在观测子流形附近重建切空间，低噪声极限下能把状态推到观测流形交集，并把不确定性限制在切空间。这里最可能的核心贡献是 invariant measurement update 上的迭代 MAP，而不是离散 IMU 模型或 landmark stacking 这些工程细节。
需要直接判断：这篇的主要增益不是 scaling / data coverage，也不是更复杂传感器模型，而是 better inductive bias + test-time compute。Iter SO(3)-EKF 已经有 test-time compute 但仍差，说明“迭代”本身不是充分条件；IEKF 已经有 invariant bias 但仍差，说明单次线性化也不是充分条件。真正有效的是二者组合。
但也不能过度外推。文中实验环境过于干净，增益是否在真实视觉 pipeline 中保持，尤其面对投影非线性、遮挡、误关联和 outlier，文中未充分说明。当前结果更像证明几何机制在理想 landmark relative-position setting 下成立，而不是证明完整 VIO 系统已经优于现有工程系统。
Relation To Prior Work
这篇属于 Barrau / Bonnabel invariant filtering 谱系和 Goffin IterIEKF 谱系的直接延伸，而不是一个全新的滤波范式。相对 Li & Mourikis 指出的 VIO false observability，它继承 IEKF 的根本修正路线：不要在错误 local coordinates 上打补丁，而是换 error geometry。相对 Brossard 等 invariant VIO/SLAM，它推进的是 measurement update 的迭代化。
和普通 iterated EKF / manifold EKF 的差别在于：那些方法主要解决非线性 measurement linearization error，不保证不可观方向的信息结构正确；本文强调迭代必须发生在 invariant error parameterization 下，否则可能只是更精确地优化一个几何上不一致的问题。
实质创新主要是把 IterIEKF 明确落到 3D landmark-aided inertial navigation，并给出 stacked multi-landmark formulation 与对比验证。看似新的部分里，SE_2(3)、右不变误差、IterIEKF 理论本身都来自已有工作；本文新增的信息更像是一个具体问题域上的机制组合与经验验证，而不是理论层面的新滤波原理。
Dataset / Evaluation
评估是 controlled simulation，而不是完整真实系统评测。作者从 EuRoC V2_01_easy 轨迹生成 ground truth inertial sequence，人工放置三个 landmark，并假设全程可见、数据关联已知、观测为三维相对位置。这个设置非常适合验证 observability + iterative invariant update 的机制，但覆盖范围窄。
实验确实支持核心 claim 的一部分：在大初始扰动、低频 landmark update、干净三维相对位置观测下，IterIEKF 比 IEKF、Iter SO(3)-EKF 和 SO(3)-EKF 更准确且 NEES 更合理。尤其 Iter SO(3)-EKF 的失败说明普通迭代不能替代 invariant geometry。
但 benchmark 没有验证若干 deployment-critical claim：没有真实 landmark detector，没有 camera projection pipeline，没有 data association uncertainty，没有地图误差，没有 outlier / occlusion，没有跨场景或多轨迹统计。所谓 MAV 场景仍主要是离线轨迹驱动仿真。评价能证明方法在理想化 landmark-aided navigation 上机制有效，不能证明它已经是现实 VIO/SLAM 系统的工程替代品。
Limitation
第一，前提强：已知地图、唯一地标 ID、所有 landmark 可见、观测模型接近直接 3D relative position。这把真实 VIO/SLAM 中最难的 perception / association / map uncertainty 移除了。方法没有解决 SLAM 的开放世界问题，只解决 localization with known landmarks。
第二，scalability 上限未充分说明。IterIEKF 每次 update 引入 Gauss-Newton 迭代，landmark 数量增长时计算和收敛行为会成为问题；文中只有 K=3 的极小 landmark set，不能说明大规模稀疏地图或高频视觉 feature 情况。
第三，bias 处理暴露结构缺陷。将 IMU bias 作为 R^6 直接附加破坏严格 group-affine property，accelerometer bias 仍不可充分估计。作者也承认 TFG 可能是更合适结构。这说明当前方法没有完全闭合 invariant navigation 的建模问题，只是对 pose/velocity/position 主体较有效。
第四，增益归因仍有边界。虽然 baseline 设计比一般论文更清楚，但 IEKF vs IterIEKF 的差距可能部分来自低噪声、强观测约束和大初始误差下的 test-time optimization 优势；在高噪声或错误观测下，迭代可能过拟合 measurement residual，文中未充分说明鲁棒性。
第五，泛化主张应谨慎。当前没有跨 dataset、跨轨迹、跨 sensor modality、跨 landmark layout 的评估；“first application to MAV”成立，但“general MAV localization advantage”证据不足。
Takeaway
- 1. 对几何滤波来说，measurement update 的几何一致性和 propagation 的 invariant error 同样重要；只做 IEKF propagation/update 的一阶版本可能还没有吃满观测流形结构。
- 2. IterIEKF 的价值在于把 EKF update 解释为低维几何约束上的局部 MAP，而不是简单 Kalman correction。
- 这一思想可以迁移到接触定位、range/bearing localization、GNSS/INS、带先验地图的多传感器融合等存在观测子流形的问题。
- 3. 仅增加迭代次数不是解决 consistency 的办法；如果误差坐标和系统对称性不匹配，迭代只会更努力地优化错误的局部模型。
一句话总结
这篇论文是将 IterIEKF 从一般 invariant filtering 理论推进到三维 landmark-aided inertial navigation 的一次机制验证，真正贡献在于把 observability-consistent invariant error 与迭代 MAP measurement update 结合起来，而不是提出新的传感器融合任务或新的 SLAM 系统。