06 · 进阶｜固定滞后因子图：把一段历史一起解释
滤波和优化的区别在于保留多少历史
滤波通常把过去的信息压缩进当前状态与协方差。因子图则把多个时刻的状态作为节点，把 IMU、DVL、深度和视觉约束作为连接它们的因子。每条观测都说“这些状态应该满足这个关系”，优化器寻找最一致的整段轨迹。
r_i 是预测与测量的残差，Σ_i 表示该残差的不确定度，ρ_i 可选鲁棒损失。相邻状态之间有运动约束，某个时刻有 DVL 速度约束，压力约束其深度。迟到的观测可以连接到相应历史状态，然后联合调整窗口内轨迹；这让时间关系在结构上更直接。
一个三时刻例子
有 x₀、x₁、x₂ 三个状态，IMU 分别约束 x₀→x₁ 和 x₁→x₂。现在收到属于 t₁ 的 DVL，不应拿它直接纠正 x₂，而是给 x₁ 加因子。优化后 x₁ 改变，由于 IMU 连接关系，x₂ 也随之改变。这与“回滚更新后重放”在思路上相通，但允许反复重线性化多种约束。
为什么使用固定窗口
长期保留全部状态会增加计算量，因此固定滞后平滑保留最近一段时间，把更早变量边缘化成先验。边缘化不是删除历史：其信息以相关先验保留。代价是旧先验通常固定在线性化点附近，过大重线性化变化和规范自由度需要认真处理。没有绝对位置锚点时，还需固定一个参考或处理零空间，而不是让优化器凭空决定世界原点。
实现与学习顺序
先实现一维 p/v 图，只含运动、速度观测和起始先验；随后加入迟到测量与固定窗口，再扩展姿态和 IMU 预积分。鲁棒损失能降低异常观测影响，但不能修复错误坐标或持续时间偏移。因子图适合多时刻、多传感器联合估计；严格低延迟的控制输出仍需及时预测，不必等待整个窗口优化结束。
学习实验：同一批延迟 DVL 数据分别用 KF 回放与固定窗口最小二乘处理，比较计算量、历史轨迹修正和当前输出延迟。阅读 GTSAM 作者的入门材料，先理解变量—因子关系，再接触求解器 API：https://gtsam.org/tutorials/intro.html 。