Skip to content
状态估计与 AI阅读约 8 分钟
Factor GraphMAP EstimationKalman FilterRobotics

从最大后验概率(MAP)出发,推导线性高斯因子图与卡尔曼滤波的等价关系,并分析滑窗优化与递推滤波在工程中的取舍。

因子图优化(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 上:

  1. 变量消元(Variable Elimination):按时间顺序从 x0 → x1 → … → xN−1 依次做 Cholesky 分解与前向消元,得到的最后一个状态 xN 的后验均值与协方差,在数值上与卡尔曼滤波递推到第 N 步的结果完全一致。
  2. 本质差异的来源:一旦 f(·) 或 h(·) 存在非线性,EKF 在每一步线性化后立即抛弃历史状态(不可逆边缘化),线性化点一旦偏差就无法修正;而 FGO 保留了一个滑动窗口内的多个历史状态,在每次迭代(Gauss-Newton / Levenberg-Marquardt)时可以重新计算雅可比矩阵并整体重线性化。
A. 递推滤波 (EKF / UKF)仅保留当前状态 x_k 与协方差 P_k,历史不可回溯x_k-1x_kz_k复杂度 O(d³) · 零历史存储B. 滑窗因子图优化 (Sliding-Window FGO)联合优化窗口内多帧状态,支持回环与延迟观测插入x_k-2x_k-1x_k跨帧约束 / 回环因子支持多轮重线性化 · 兼容非高斯鲁棒核 (Huber / Cauchy)

3. 工程选型对照表 ​

维度EKF / UKF(递推滤波)Sliding-Window FGO(因子图)
线性化方式单步线性化(或 Sigma 点传播),不可回溯窗口内多轮迭代重线性化,收敛域更大
乱序 / 延迟观测需维护环形状态队列做回滚重算原生支持按时间戳插入对应历史节点
非高斯野值鲁棒性依赖马氏距离门控(硬截断)可直接嵌入 Huber / Cauchy 鲁棒核函数(软降权)
端侧算力与时延极低且确定性高(微秒级),适合高频控制闭环随窗口长度增长,通常运行在 10–50 Hz 后端线程

工程实践建议:在高频里程计与底层控制闭环(200 Hz–1 kHz)中采用 EKF / UKF 承担前向递推,而在中低频多源融合(如 GNSS、相机回环、4D 雷达跨帧配准)中采用 FGO 做后端滑窗校正,两者通过固定步长状态增量耦合,是兼顾确定性时延与全局精度的主流方案。

讨论区开启了自动与人工双重审核,文明交流的评论会在短时间内展示。

Built with VitePress