主题
因子图优化(FGO)与递推卡尔曼滤波的数学等价性与工程权衡
在机器人导航与多传感器状态估计中,递推卡尔曼滤波(KF / EKF / UKF) 与 因子图优化(Factor Graph Optimization, FGO) 常被视为两条不同的技术路线。实际上,在线性高斯假设下,两者求解的是同一个最大后验概率(MAP)问题;真正的分歧在于非线性重线性化能力与边缘化(Marginalization)策略。
1. 从 MAP 目标函数到线性最小二乘
设系统在离散时间窗口 k = 0, …, N 内的状态轨迹为 𝒳 = { x0, …, xN },观测量集合为 𝒵 = { z1, …, zN }。假设过程模型与观测模型均服从高斯噪声:
MAP Estimation Objective
𝒳* = arg min𝒳 ( ||x0 − μ0||2Σ0 + Σk=1..N ||xk − f(xk−1)||2Qk + Σk=1..N ||zk − h(xk)||2Rk )(1)
其中 ||e||2Σ = eT Σ−1 e 为马氏距离平方。当 f(·) 与 h(·) 为线性算子时,上述目标函数是严格的凸二次型,其正规方程 JT J δ𝒳 = −JT r 的系数矩阵 Λ = JT J(信息矩阵)呈块三对角稀疏结构。
2. 为什么递推卡尔曼滤波等价于“单步消元”?
在因子图中,对历史状态 xk−1 进行舒尔补(Schur Complement)消元,相当于把先验信息与过程因子压缩为一个新的高斯先验因子作用在 xk 上:
- 变量消元(Variable Elimination):按时间顺序从 x0 → x1 → … → xN−1 依次做 Cholesky 分解与前向消元,得到的最后一个状态 xN 的后验均值与协方差,在数值上与卡尔曼滤波递推到第 N 步的结果完全一致。
- 本质差异的来源:一旦 f(·) 或 h(·) 存在非线性,EKF 在每一步线性化后立即抛弃历史状态(不可逆边缘化),线性化点一旦偏差就无法修正;而 FGO 保留了一个滑动窗口内的多个历史状态,在每次迭代(Gauss-Newton / Levenberg-Marquardt)时可以重新计算雅可比矩阵并整体重线性化。
3. 工程选型对照表
| 维度 | EKF / UKF(递推滤波) | Sliding-Window FGO(因子图) |
|---|---|---|
| 线性化方式 | 单步线性化(或 Sigma 点传播),不可回溯 | 窗口内多轮迭代重线性化,收敛域更大 |
| 乱序 / 延迟观测 | 需维护环形状态队列做回滚重算 | 原生支持按时间戳插入对应历史节点 |
| 非高斯野值鲁棒性 | 依赖马氏距离门控(硬截断) | 可直接嵌入 Huber / Cauchy 鲁棒核函数(软降权) |
| 端侧算力与时延 | 极低且确定性高(微秒级),适合高频控制闭环 | 随窗口长度增长,通常运行在 10–50 Hz 后端线程 |
工程实践建议:在高频里程计与底层控制闭环(200 Hz–1 kHz)中采用 EKF / UKF 承担前向递推,而在中低频多源融合(如 GNSS、相机回环、4D 雷达跨帧配准)中采用 FGO 做后端滑窗校正,两者通过固定步长状态增量耦合,是兼顾确定性时延与全局精度的主流方案。