简介本资源是一个面向控制工程、机器人导航与传感器融合领域的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 概念因子图对应物运动模型二元运动因子约束相邻时刻变量测量模型一元测量因子约束当前时刻变量先验分布先验因子约束初始时刻变量雅可比矩阵因子残差对变量状态的导数协方差传递高斯牛顿迭代中的信息矩阵状态更新变量节点值的修正量这样对照着看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_{k1}, v_{k1}, 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 后更新变量重复迭代直到收敛。我没有一上来就写 LMLevenberg-Marquardt而是先做高斯牛顿原因有两个。第一代码包的核心目标是让使用者理解因子图的结构和推理过程高斯牛顿结构最清晰。第二绝大多数因子图问题只要初始化不太离谱高斯牛顿已经足够LM 只是在阻尼项上有改进理解了高斯牛顿加 LM 只是多十几行代码的事。在实际使用中如果遇到发散先检查雅可比和初始化比盲目换 LM 有效得多。2.4 代码包的使用流程使用这套代码包的典型流程创建图对象fg.FactorGraph()添加变量节点addVariable(obj, id, initValue)添加因子addFactor(obj, factor)调用求解器solver.solve(graph, maxIter)提取结果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 objfg.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_{k1}。假设匀速运动模型x_{k1} x_k v_k * dt 0.5 * u_k * dt^2 v_{k1} v_k u_k * dt残差定义为预测值与状态之差r_motion [x_{k1} - (x_k v_k * dt 0.5 * u_k * dt^2); ... v_{k1} - (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:startRowvarNodes{j}.dim-1, ... startCol:startColvarNodes{k}.dim-1) ... H(startRow:startRowvarNodes{j}.dim-1, ... startCol:startColvarNodes{k}.dim-1) ... Js{j} * Omega * Js{k}; end g(startRow:startRowvarNodes{j}.dim-1) ... g(startRow:startRowvarNodes{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 个时刻的变量节点新测量到达时把最老的时刻边缘化去掉。边缘化在因子图里是一个有点技巧的操作。简单做法是直接丢弃老变量但这会损失信息。更严谨的做法是生成一个边缘化因子把老变量的信息“投影”到剩余变量上。我在代码包里实现了丢弃式滑窗因为对教学场景来说理解滑窗本身比边缘化算法更重要。真要上工程可以在这个基础上再扩展边缘化模块。滑窗更新流程新测量到达创建新变量节点和测量因子删除超出窗口长度的变量节点及其关联因子重新执行高斯牛顿求解这样做每次迭代只处理窗口内的变量实时性比全量批量好很多而且和 EKF 的“只维护当前状态”思路更接近。4.3 因子图与 EKF 的结果对比我在一维定位例子里做了对比因子图批量平滑和 EKF 在相同的仿真数据上跑。结论符合预期指标EKF因子图批量平滑定位平均误差0.32 m0.18 m速度平均误差0.15 m/s0.09 m/s是否能利用未来测量回修正否是是否支持多假设约束不支持支持计算复杂度O(n) 随时刻递推O((W·d)^3)W 为窗口大小代码可扩展性中高这个对比不是黑 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(:, k1) [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, k1, 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 或者对接深度学习框架都会顺畅很多。本文还有配套的精品资源点击获取