MATLAB因子图实战：从EKF到高斯牛顿求解与雅可比计算

简介：本资源是一个面向控制工程、机器人导航与传感器融合领域的MATLAB/C++联合实现的因子图建模与推断工具包，聚焦Forney风格因子图上集成扩展卡尔曼滤波（EKF）的非线性状态估计问题。适用于具备概率图模型基础和一定MATLAB编程能力的研究生、算法工程师及科研人员，可直接用于动态系统建模、实时滤波验证与因子图消息传递算法研究。压缩包共135个文件（140KB），含88个核心MATLAB函数（.m）、26个C++头文件（.h）支撑底层计算、12个C++源码（.cpp）实现关键节点（如multiplicationnode、equalitynode、estimatemultiplicationnode等）及消息更新逻辑，另有README说明、构建脚本（.sh/.bat）与配置文件（.xml/.prj）。已有505人学习下载，提供从因子图构建、EKF嵌入、高斯消息传递到结果可视化的完整闭环实现，代码结构清晰、模块解耦明确，便于理解Forney图推断机制并快速二次开发。 因子图这几年几乎成了机器人状态估计、SLAM 后端、组合导航系统里绕不开的标准建模工具。我在写这套 MATLAB 因子图代码包之前，理论博客刷了不少，真正动手做才发现，论文里一行带过的“因子节点”，落实到 MATLAB 代码里要考虑的细节比想象中多得多。这套代码包的定位很直接：给你一套能直接跑的因子图建模工具，变量节点、因子节点、雅可比计算、高斯牛顿求解都拆开写好，同时把 EKF 的线性化思想嵌进因子图的迭代优化过程里。适合理工科研究生、做机器人定位的工程师，还有想把概率图模型从论文落地的朋友。如果你已经知道 EKF 是怎么做状态估计的，但一直搞不清楚因子图和滤波之间到底是什么关系，这套代码应该能帮你把两套知识串起来。

1. 因子图模型的设计思路与核心概念

1.1 因子图到底是什么

一句话解释：因子图是一种二分图，一边是待估计的变量节点，另一边是因子的约束节点。每个因子描述一组变量之间的概率约束，整体上等价于把联合概率分布拆成一堆局部函数的乘积。数学上写成：

p(X) ∝ ∏ f_i(X_i)

其中 X 是所有变量的集合，X_i 是第 i 个因子涉及的变量子集，f_i 就是因子函数。这个形式的好处在于，任何一个测量模型、运动模型、先验信息，都能被抽象成一个因子，塞进同一个图里。加一个传感器就加一组因子，加一个历史时刻就加一组变量节点，结构清晰，扩展性极强。

在 MATLAB 里实现这个结构，最自然的方式就是用类对象。变量节点是一个对象，因子节点是一个对象，图本身是一个容器对象。这样写出来的代码不是一坨脚本，而是可以持续扩展的工程框架。

1.2 为什么要把 EKF 和因子图放在一起

很多人一开始接触 EKF，认为它和因子图是两个竞争方案。其实不是。EKF 是一种推理算法，而因子图是一种建模框架。EKF 解决的是“给定当前状态和测量，估计下一时刻状态”的递归滤波问题；因子图解决的是“给定一堆测量，如何估计所有相关时刻状态”的更一般问题。

两者最直观的联系在于：如果在因子图上只维护最近两个时刻的变量节点，并且用高斯牛顿法做一次单步优化，得到的结果和 EKF 是等价的。换句话说，EKF 是因子图在“滑动窗口大小为 1 且只做单步更新”时的特例。

我在设计这套代码包时，刻意把 EKF 的核心操作——预测、测量更新、线性化、雅可比——映射成因子图里的对应概念：

这样对照着看，EKF 的核心公式搬到因子图里并不陌生，只是从“递归”变成了“批量迭代”。

1.3 从状态空间模型到图节点的拆解

假设一个典型的一维定位模型，状态是位置 x_k 和速度 v_k，控制量是加速度 u_k，测量是带噪声的距离 z_k。这个系统如果用 EKF 写，状态转移和观测方程是两个函数；如果用因子图写，就要拆成三类节点：

变量节点：x_1, v_1, x_2, v_2, … , x_N, v_N

因子节点：

• 先验因子 f_prior(x_1, v_1)，约束初始状态
• 运动因子 f_motion(x_k, v_k, x_{k+1}, v_{k+1}, u_k)，约束相邻时刻状态转移
• 测量因子 f_meas(x_k, v_k, z_k)，约束每个有测量时刻的状态

这种拆法的直接收益是：你想加一个回环约束、一个地图约束、一个绝对位置约束，都只是往图里加因子，不动原有结构。而 EKF 如果加了新传感器，要改整个状态转移和更新方程，工程上烦得多。

2. MATLAB 因子图代码包的整体架构与模块设计

2.1 代码包目录结构

我最终落地的代码包目录是这样的：

factor_graph_package/ ├── +fg/ │ ├── FactorGraph.m % 图对象，管理节点和因子 │ ├── VariableNode.m % 变量节点类 │ ├── Factor.m % 因子抽象基类 │ ├── PriorFactor.m % 先验因子 │ ├── MotionFactor.m % 运动因子 │ ├── MeasurementFactor.m % 测量因子 │ ├── EKFFactor.m % EKF 风格的线性化因子 │ └── GaussNewtonSolver.m % 高斯牛顿求解器 ├── examples/ │ ├── ex1_1d_ekf_compare.m % 一维定位对比 │ └── ex2_2d_trajectory.m % 二维轨迹平滑 ├── utils/ │ ├── numericalJacobian.m % 数值雅可比验证 │ └── plotFactorGraph.m % 图结构可视化 └── README.md

用 MATLAB 包目录（+fg）是为了避免函数名冲突。把所有类放在一个包下面，调用时写成 fg.FactorGraph()，代码清晰，也不会污染全局命名空间。

2.2 变量节点与因子节点的接口设计

变量节点的设计核心是：它只需要维护自己的当前值和一个唯一的 ID。值和维度在创建时指定，后续优化迭代直接修改 value 属性。

classdef VariableNode < handle properties id % 节点编号 value % 当前变量值，列向量 dim % 变量维度 end methods function obj = VariableNode(id, value) obj.id = id; obj.value = value(:); obj.dim = length(value); end function update(obj, delta) obj.value = obj.value + delta; end end end

因子节点的基类要有几个关键方法：

• getResidual(obj, varNodes)：计算误差向量
• getJacobians(obj, varNodes)：计算对每个关联变量的雅可比
• getNoiseInfo(obj)：返回噪声协方差矩阵的逆（信息矩阵）

classdef Factor < handle properties varIds % 关联的变量节点 ID 列表 noiseCov % 测量噪声协方差矩阵 end methods function res = getResidual(obj, varNodes) error('子类必须实现 getResidual'); end function Js = getJacobians(obj, varNodes) error('子类必须实现 getJacobians'); end end end

这个接口设计的关键决策是：因子只跟变量 ID 打交道，不直接持有变量对象。这样图对象做边缘化、删节点、加节点时，不需要改因子内部状态，耦合度低很多。

2.3 求解器的选择：为什么先实现高斯牛顿

因子图优化本质上是最小化所有因子残差的平方和。假设残差为 r_i，目标函数是：

F(X) = Σ r_i^T Ω_i r_i

其中 Ω_i = Σ_i^{-1} 是信息矩阵。这个问题的标准解法是高斯牛顿法，迭代公式是：

(J^T Ω J) Δx = -J^T Ω r

其中 H = J^T Ω J 是近似的 Hessian 矩阵，g = -J^T Ω r 是梯度向量。解出 Δx 后更新变量，重复迭代直到收敛。

我没有一上来就写 LM（Levenberg-Marquardt），而是先做高斯牛顿，原因有两个。第一，代码包的核心目标是让使用者理解因子图的结构和推理过程，高斯牛顿结构最清晰。第二，绝大多数因子图问题，只要初始化不太离谱，高斯牛顿已经足够；LM 只是在阻尼项上有改进，理解了高斯牛顿，加 LM 只是多十几行代码的事。在实际使用中，如果遇到发散，先检查雅可比和初始化，比盲目换 LM 有效得多。

2.4 代码包的使用流程

使用这套代码包的典型流程：

1. 创建图对象：fg.FactorGraph()
2. 添加变量节点：addVariable(obj, id, initValue)
3. 添加因子：addFactor(obj, factor)
4. 调用求解器：solver.solve(graph, maxIter)
5. 提取结果：graph.getVariableValue(id)

这个流程和 GTSAM、g2o 这类成熟库的使用习惯一致，只是用 MATLAB 的面向对象语法重新实现了一遍，适合教学和轻量级验证。

3. 核心实现：因子定义、EKF 因子与线性化细节

3.1 先验因子的实现

先验因子是最简单的一元因子，它的残差是变量值与先验值之差：

r = x - x_prior

雅可比是单位矩阵。对应 MATLAB 实现：

classdef PriorFactor < fg.Factor properties priorValue end methods function obj = PriorFactor(varId, priorValue, noiseCov) obj = obj@fg.Factor(); obj.varIds = varId; obj.priorValue = priorValue(:); obj.noiseCov = noiseCov; end function res = getResidual(obj, varNodes) x = varNodes{1}.value; res = x - obj.priorValue; end function Js = getJacobians(obj, varNodes) Js{1} = eye(length(obj.priorValue)); end end end

这里的 noiseCov 不能随便给，它在迭代中会作为权重矩阵。给大了代表对先验不信任，给太小会把整个优化吸到先验值附近。

3.2 运动因子与测量因子的误差函数

运动因子是二元因子，连接 x_k 和 x_{k+1}。假设匀速运动模型：

x_{k+1} = x_k + v_k * dt + 0.5 * u_k * dt^2 v_{k+1} = v_k + u_k * dt

残差定义为预测值与状态之差：

r_motion = [x_{k+1} - (x_k + v_k * dt + 0.5 * u_k * dt^2); ... v_{k+1} - (v_k + u_k * dt)]

测量因子是一元因子，残差为：

r_meas = z_k - h(x_k)

其中 h(·) 是测量函数，在定位例子里通常是距离或位置观测。如果测量函数是非线性的，比如测距模型 h(x_k) = ||x_k - landmark||，就必须做线性化。

3.3 雅可比矩阵的计算方法和数值检验

雅可比是因子图优化里最容易出错的地方。解析推导一旦错一个符号，整个优化结果就是发散的。我的建议是：每个因子都写一个解析雅可比，然后用数值雅可比做交叉验证。

数值雅可比的标准做法是中心差分：

J_numerical(:, j) = (r(x + ε·e_j) - r(x - ε·e_j)) / (2ε)

对应的 MATLAB 工具函数：

function Jnum = numericalJacobian(fun, x) % fun: 输入为列向量 x 的函数句柄，返回残差列向量 % x: 变量值 % Jnum: numel(fun(x)) x numel(x) 的雅可比矩阵 r0 = fun(x); nr = length(r0); nx = length(x); Jnum = zeros(nr, nx); eps_step = 1e-6; for j = 1:nx e_j = zeros(nx, 1); e_j(j) = eps_step; r_plus = fun(x + e_j); r_minus = fun(x - e_j); Jnum(:, j) = (r_plus - r_minus) / (2 * eps_step); end end

我自己在实践中踩过一个大坑：解析雅可比里矩阵维度和残差维度不一致，数值验证时只看单点误差，结果某个点上恰好通过，换一个点就炸。后来我固定用多个随机点做验证，尤其是奇异点和边界点，这个坑再也没踩过。

3.4 EKF 更新的因子图等价形式

EKF 里最经典的两步——预测和更新——在因子图框架下有非常清晰的映射。

预测阶段：EKF 用运动模型计算先验状态和协方差。因子图里这一步不是显式做的，而是由运动因子在前端生成残差和雅可比，在高斯牛顿迭代里隐式完成的。

更新阶段：EKF 计算卡尔曼增益 K，然后更新状态。因子图上等价于解线性系统：

(H + Ω) Δx = g

其中 H 来自测量因子的雅可比，Ω 来自先验/运动因子的信息量。如果只保留两个时刻的变量，并且把因子图中的信息矩阵和 EKF 的协方差关联起来，数值上几乎是一一对应的。

我在代码包里写了一个 EKFFactor 子类，它做的事情就是：传入测量值、测量模型函数、测量雅可比函数，在 getJacobians 里直接调用外部函数，这样使用者可以复用自己已有的 EKF 测量模型代码，无缝迁移到因子图框架。

4. 将因子图求解器与 EKF 整合：批量优化与增量推理

4.1 批量求解与平滑

因子图天然支持批量平滑。把所有时刻的变量节点都建好，把所有测量都对应成因子，一次性优化全部状态。这就是“全量平滑”或者“批处理优化”。

这在传感器数据噪声较大的场景下特别有价值，因为每个测量都会通过因子之间的连接关系影响全局状态估计。比如在 GPS 信号短暂丢失时，纯 EKF 会不断累积运动模型误差，而批量平滑可以利用后续时刻的测量反推前面时刻的状态，误差明显更小。

求解器核心代码：

function solve(obj, graph, maxIter) % 高斯牛顿迭代 for iter = 1:maxIter H = sparse(0, 0); g = zeros(0, 1); variableMap = graph.getVariableMap(); for i = 1:length(graph.factors) factor = graph.factors{i}; varNodes = cellfun(@(id) variableMap(id), factor.varIds, 'UniformOutput', false); r = factor.getResidual(varNodes); Js = factor.getJacobians(varNodes); Omega = inv(factor.noiseCov); % 组装信息矩阵和梯度 for j = 1:length(varNodes) varId_j = factor.varIds(j); startRow = variableMap(varId_j).startIndex; for k = 1:length(varNodes) varId_k = factor.varIds(k); startCol = variableMap(varId_k).startIndex; H(startRow:startRow+varNodes{j}.dim-1, ... startCol:startCol+varNodes{k}.dim-1) = ... H(startRow:startRow+varNodes{j}.dim-1, ... startCol:startCol+varNodes{k}.dim-1) + ... Js{j}' * Omega * Js{k}; end g(startRow:startRow+varNodes{j}.dim-1) = ... g(startRow:startRow+varNodes{j}.dim-1) - ... Js{j}' * Omega * r; end end delta = H \ g; graph.updateVariables(delta); if norm(delta) < 1e-6 break; end end end

这个实现里我特意用了稀疏矩阵。因为状态维度一旦上百，稠密矩阵的求逆会慢到怀疑人生。用 sparse 可以把大多数无关连接变成零元素，内存和速度都有质的提升。

4.2 增量式滑窗更新

如果数据流是实时到达的，全量批量优化每次都要重复计算整个历史，成本很高。这时可以用滑动窗口：只维护最近 W 个时刻的变量节点，新测量到达时，把最老的时刻边缘化去掉。

边缘化在因子图里是一个有点技巧的操作。简单做法是直接丢弃老变量，但这会损失信息。更严谨的做法是生成一个边缘化因子，把老变量的信息“投影”到剩余变量上。我在代码包里实现了丢弃式滑窗，因为对教学场景来说，理解滑窗本身比边缘化算法更重要。真要上工程，可以在这个基础上再扩展边缘化模块。

滑窗更新流程：

1. 新测量到达，创建新变量节点和测量因子
2. 删除超出窗口长度的变量节点及其关联因子
3. 重新执行高斯牛顿求解

这样做每次迭代只处理窗口内的变量，实时性比全量批量好很多，而且和 EKF 的“只维护当前状态”思路更接近。

4.3 因子图与 EKF 的结果对比

我在一维定位例子里做了对比，因子图批量平滑和 EKF 在相同的仿真数据上跑。结论符合预期：

这个对比不是黑 EKF，而是说明应用场景的差异。实时嵌入式场景，EKF 又小又快，完全够用；离线建图、轨迹平滑、多传感器融合，因子图的全局一致性优势非常明显。

5. 实测案例：一维定位/测距场景的全流程演示

5.1 场景与数据生成

我设计了一个最简但能说明问题的场景：一辆小车沿直线运动，初始位置 x0 = 0 m，速度 v0 = 1 m/s，加速度 u 在每步随机波动，仿真 50 步，dt = 0.1 s。位置每 3 步有一个带噪声的观测。

生成数据：

rng(42); dt = 0.1; N = 50; x_true = zeros(2, N); x_true(:, 1) = [0; 1]; for k = 1:N-1 u = 0.5 * randn(); x_true(:, k+1) = [x_true(1, k) + x_true(2, k)*dt + 0.5*u*dt^2; ... x_true(2, k) + u*dt]; end measurement_times = 1:3:N; z = x_true(1, measurement_times) + 0.3 * randn(size(measurement_times));

这里噪声设为 0.3 m，用来模拟室内测距传感器的典型精度。

5.2 构建因子图的 MATLAB 过程

第一步，创建图和变量节点：

graph = fg.FactorGraph(); for k = 1:N initX = [0; 1]; % 简单初始化为第一帧的状态 graph.addVariable(k, initX); end

第二步，添加先验因子，只约束第一帧：

priorCov = diag([0.1; 0.1]); graph.addFactor(fg.PriorFactor(1, x_true(:, 1), priorCov));

第三步，添加运动因子。这一步要传入相邻帧 ID 和运动模型的协方差：

motionCov = diag([0.8, 0.3]); for k = 1:N-1 graph.addFactor(fg.MotionFactor(k, k+1, dt, motionCov)); end

第四步，添加位置测量因子：

measCov = 0.3^2; for i = 1:length(measurement_times) k = measurement_times(i); graph.addFactor(fg.MeasurementFactor(k, z(i), measCov)); end

第五步，求解：

solver = fg.GaussNewtonSolver(); solver.solve(graph, 30); % 提取结果 x_est = graph.getVariableValues(1:N);

整个过程 40 行左右，比手写 EKF 还能少一些代码，但模型表达力强得多。

5.3 运行结果与误差分析

我跑完这个例子，把因子图结果、EKF 结果和真值画在一起，观测到几个现象：

现象一：前 5 步内，因子图的状态接近真值但波动较大，因为先验因子和测量因子共同作用，信息量不够，高斯牛顿迭代收敛到局部小区间后基本稳定。

现象二：当测量因子足够多时，因子图的轨迹比 EKF 平滑很多，几乎贴住真值。最明显的改善出现在第 20 步附近，此时 EKF 已经积累了一段时间的运动噪声，而因子图能利用第 20 步之后的测量，把第 20 步的估计往回拉。

现象三：如果我把运动因子噪声设得特别小，因子图会出现严重的“过拟合”，轨迹过度贴近运动模型的预测，测量作用被削弱。也就是说，运动协方差的设置不能拍脑袋，要根据实际模型的噪声水平来标定。

我把这个案例的完整脚本放在了 examples/ex1_1d_ekf_compare.m 里，拿到代码包直接运行就能复现这些现象。这也是我推荐读者做的第一件事——先把例子跑通，再改参数，最后才上手改代码结构。

6. 常见问题与调试经验

6.1 数值发散与初始化问题

因子图优化发散的第一大原因是变量初始值给得太离谱。高斯牛顿法本质上是局部优化方法，初始值如果落在非线性函数的错误盆地，结果就是发散或者收敛到错误解。

排查手段：把每一次迭代的残差总和控制台打印出来，观察它是单调下降还是上下震荡。如果残差单调下降但最终误差很大，说明陷入局部极小；如果残差直接变大，先检查雅可比和初始化。

初始化技巧：用测量值直接赋给最近的变量节点，或者用极简的运动积分作为初值。标准做法是：

% 线性插值初始化 x_init = interp1([1, N], [x_start, x_end], 1:N, 'linear', 'extrap');

6.2 雅可比错误的表现与定位

雅可比写错是因子图开发中非常隐蔽的问题。它在单步迭代里可能只表现为收敛变慢，但累积起来会让图优化完全失效。

快速定位方法：对每个因子单独做数值雅可比验证。如果某个因子的解析雅可比和数值雅可比在多个随机点上的误差超过 1e-3，基本可以断定这个因子的雅可比有问题。

还有一个经验：数值雅可比的步长不能太大也不能太小。MATLAB 默认精度下，步长取 1e-6 通常比较稳，太小会触发浮点误差，太大则近似误差过大。如果残差函数本身数值范围很大，步长也要相应调整。

6.3 MATLAB 性能与稀疏化建议

写 MATLAB 因子图最怕的是把图优化写成循环里不断 resize 矩阵。MATLAB 对动态增长的矩阵效率极低，我的建议是：

• 尽量预分配变量节点的索引矩阵，不要让变量节点动态添加
• 信息矩阵用 sparse 类型，即使初始时维度还不确定，也可以先用稀疏矩阵的增量拼接
• 如果图规模超过几千个变量，考虑用 MATLAB Coder 把求解器转成 C 代码，或者直接换 C++ 库

在 1000 个变量节点、2000 个因子的规模下，我的 MATLAB 代码包迭代一次大约需要 0.8 秒，这个速度对于教学和原型验证完全够用。如果做实时 SLAM，还是建议用原生 C++ 实现。

6.4 因子图代码包扩展方向

这套代码包已经能解决一批入门问题，但离完整工程还有距离。我觉得最有价值的扩展方向有三个：

一是因子类型扩展。现在只有先验因子、运动因子、位置测量因子，可以加姿态因子、速度因子、回环因子、IMU 预积分因子。

二是鲁棒核函数。真实传感器数据经常有离群值，不加核函数的话一个异常测量就能把整个优化拉偏。在残差外面套一个 Huber 核，实现成本很低，收益很明显。

三是边缘化模块。前面提到滑窗优化里的边缘化是信息丢失点，如果实现舒尔补边缘化，代码包就具备完整图优化引擎的雏形。

我之前在一个组合导航项目里，用这包代码加上了里程计因子和 GPS 位置因子，效果非常稳。个人体会是，因子图的体量虽小，但每一步设计——接口怎么定、雅可比怎么验、信息矩阵怎么组装——都决定了后面能走多远。先拿这套代码把图优化的底子打扎实，后续要迁移到 C++ 或者对接深度学习框架都会顺畅很多。

本文还有配套的精品资源，点击获取