简介因子图模型构建完整代码包以MATLAB为主、C接口为辅面向从事概率建模、状态估计与融合算法研究的工程师和科研人员。该包实现了Forney风格因子图上的消息传递推断并将扩展卡尔曼滤波EKF作为因子节点集成适用于非线性动态系统的实时状态更新可迁移至导航、传感器融合、机器人定位等应用场景。资源共135个文件包含88个.m脚本因子图构建、增删节点、推断与可视化、26个.h与12个.cppmexfactorgraph、multiplicationnode、equalitynode等核心C实现及MATLAB接口另有md/readme说明、mex编译脚本、MAT工程文件与示例数据整体约140KB结构清晰紧凑。目前已有507人学习下载。借助这套框架读者能快速搭建自定义因子图复用EKF与高斯分布因子在MATLAB中完成算法调试和结果绘图再通过C接口部署到性能敏感的计算环境是理解因子图推断与EKF结合的良好起点。1. 因子图与EKF同一个状态估计问题为什么我推荐换成因子图拿到一份MATLAB写的因子图模型构建完整代码包时别急着当黑匣子去跑。我在组合导航项目里被EKF的协方差更新折腾了大半年才下决心把手里的惯导/里程计/伪距融合方案全部迁移到因子图优化上。原因是当你只有一对IMU和编码器时EKF非常顺手一旦加入视觉约束、回环检测、车道线观测且观测时间戳完全异步时EKF的雅可比串行更新、协方差传播和状态扩维逻辑会膨胀到难以维护。而因子图把整个估计问题重新表述成“变量节点 因子节点”的最小二乘结构增删一个观测只是往图里塞一条残差边求解器自动处理信息耦合改动点非常集中。这份代码包的核心价值是用MATLAB把因子图的构建、EKF线性化残差的接入、以及迭代求解器串成一条完整链路。它适合正在做组合导航、SLAM前端/后端、多传感器融合的工程师也适合读论文时对gtsam、g2o里的因子图概念只知其名、想在自己数据集上复现的科研党。下面我会按照“图怎么搭 → 求解器怎么写 → 和EKF怎么配合 → 哪些地方必踩坑”的顺序把整个方案拆给你看。2. 从EKF到因子图先搭变量节点与因子节点再谈优化2.1 因子图里的三类节点与EKF的对应关系因子图其实不是一个高深的新理论它就是把你已经熟悉的EKF公式重新画成一张二分图。图的左边是一组变量节点对应你要估计的状态比如k时刻的位置、速度、姿态偏置图的右边是一组因子节点对应约束这些状态的信息源比如先验、运动模型、观测模型。一条边连接一个因子和若干个它涉及的变量表示“我手里有一个关于这些变量的残差期望它们能满足某个分布”。在EKF里你维护的是状态向量x和协方差P用预测步做状态传播用更新步做增益加权。在因子图里这些步骤被统一成一项项残差代价先验因子把初始状态固定住数学上等价于EKF里状态初值和P0的注入。运动因子由IMU预积分、编码器里程计或恒速模型生成连接相邻两个时刻的状态对应EKF预测步里的动态方程。观测因子把GPS、伪距、视觉路标等观测约束到当前状态上对应EKF的观测更新。所有因子的联合概率最大化等价于对残差做最小二乘。这里的“图”只是直观显示哪些变量之间有关联真正计算时你需要做的是把每个因子产生的残差和雅可比矩阵递给一个非线性优化器。2.2 用MATLAB类定义变量与因子先写出数据结构一份可扩展的因子图代码包第一步不是优化器而是把数据结构和EKF的向量/矩阵分开。我推荐用handle类去写因为因子图在迭代求解过程中要反复访问和修改变量节点的值值对象按引用传递能省掉大量拷贝开销。classdef FactorGraph handle properties variables % 变量节点数组: struct(id, value, dim, fixed) factors % 因子节点数组: struct(type, varIds, obs, cov, noiseModel) numVar % 变量个数 numFactor % 因子个数 end methods function addVariable(fg, id, value, dim, isFixed) % 添加一个变量节点 % id: 自定义索引value: 初值dim: 状态维度 % isFixed: 1表示固定该变量, 用于锚定第一个状态 fg.numVar fg.numVar 1; var struct(id, id, value, value(:), ... dim, dim, fixed, isFixed); fg.variables [fg.variables; var]; end function addFactor(fg, type, varIds, obs, cov) % 添加一个因子 % type: prior / motion / gps % varIds: 该因子关联的变量id数组 % obs: 观测值向量; cov: 观测噪声协方差矩阵 fg.numFactor fg.numFactor 1; factor struct(type, type, varIds, varIds, ... obs, obs(:), cov, cov, ... sqrtInfo, chol(inv(cov), upper)); fg.factors [fg.factors; factor]; end function x getStateVector(fg) % 把所有自由变量拼接成一个大向量, 供优化器使用 x []; for i 1:fg.numVar if ~fg.variables(i).fixed x [x; fg.variables(i).value]; end end end function setStateVector(fg, x) % 把优化器算出的新向量拆回各变量节点 idx 1; for i 1:fg.numVar if ~fg.variables(i).fixed dim fg.variables(i).dim; fg.variables(i).value x(idx : idxdim-1); idx idx dim; end end end end end这段代码的逻辑说明addVariable里用id做外部索引解决求解时变量维度变化的问题fixed标记用于把第一个位姿或航向锚定否则整个最小二乘问题是欠约束的矩阵会奇异。addFactor里我预先做了chol(inv(cov))得到上三角平方根信息矩阵后面残差会用它做白化这样不需要在每次迭代里重复分解协方差能省掉不少开销。参数说明cov必须是方阵维度等于观测向量长度如果观测值尺度差很大比如角度是0.1弧度级、距离是100米级记得把cov设成对角矩阵对角线按量测噪声方差填不要拍脑袋取1。isFixed一旦设为1该变量就不会出现在优化器的自由向量里但残差计算仍然会用到它这是锚定的标准做法。2.3 批量版本在做什么一个三变量两因子的最小例子为了不被大框架淹没先看一个只有三个变量、两个因子的小例子变量x1、x2、x3一维状态一个先验因子固定x10一个运动因子约束x1-x2的增量一个观测因子约束x2-x3的增量。这种最小问题能让你看到因子图求解的本质——它在解一个带初始猜测的线性方程组。fg FactorGraph(); fg.addVariable(0, 0.0, 1, true); % x1 固定为0 fg.addVariable(1, 1.0, 1, false); % x2 fg.addVariable(2, 0.2, 1, false); % x3 % 先验: x1 应等于 0, 协方差 0.01 fg.addFactor(prior, [0], 0.0, diag(0.01)); % 运动: x2 - x1 1.5, 协方差 0.04 fg.addFactor(motion, [0, 1], 1.5, diag(0.04)); % 观测: x3 - x2 -1.0, 协方差 0.09 fg.addFactor(motion, [1, 2], -1.0, diag(0.09));这个例子里prior因子连接变量[0]motion因子连接[0,1]和[1,2]。求解后x2会接近1.5x3接近0.5但由于三者的协方差比例不同最终值会在先验、运动约束、观测约束之间做一个加权折中而不是严格满足后两条。这一点正是因子图和EKF的本质区别EKF的更新是串行的后一个观测会覆盖前一个观测的一部分影响因子图把全部约束一次性求最优信息不会因加入顺序而改变。3. 手写一个可用的因子图求解器残差、雅可比与LM迭代3.1 高斯牛顿和LM的核心线性化与增量方程因子图的代价函数是所有残差的平方和信息矩阵加权和。要最小化它最常见的是高斯牛顿法在每次迭代中把残差r(x)线性化为r(xdx) ≈ r(x) J * dx然后求解正规方程(J^T * W * J) * dx -J^T * W * r(x)。这里的W就是我在上一章里预先算好的sqrtInfo的平方。增量dx求出后更新状态x ← x dx重复直到收敛。高斯牛顿在因子图里有个退化风险当J^T W J奇异或条件数很大时增量会剧烈跳变。所以我一般都直接在因子图求解器里用Levenberg–MarquardtLM在高斯牛顿方程的对角线上加一个阻尼项λ * diag(J^T W J)方向介于高斯牛顿和梯度下降之间。λ小的时候偏向二次收敛大的时候稳定下滑。下面是LM的骨架。function [x, iter] lmSolve(fg, x0, maxIter, tol, lambda0) % 因子图LM求解器 % x0: 初始状态向量; maxIter: 最大迭代次数 % tol: 增量范数收敛阈值; lambda0: 阻尼初值 x x0; lambda lambda0; for iter 1:maxIter fg.setStateVector(x); % 计算整体残差向量和雅可比矩阵 [r, J] computeResidualAndJacobian(fg); % 正规方程: H J^T * W * J, b -J^T * W * r W assembleWeightMatrix(fg); % 块对角矩阵 H J * W * J; b -J * W * r; % 检查当前残差平方和 cost 0.5 * r * W * r; % LM: 给H的对角线加阻尼 H_lm H lambda * diag(diag(H)); % 求解增量 dx H_lm \ b; % 尝试更新, 看代价是否下降 x_new x dx; fg.setStateVector(x_new); [r_new, ~] computeResidualAndJacobian(fg); cost_new 0.5 * r_new * W * r_new; if cost_new cost % 接受更新, 减小阻尼 x x_new; lambda lambda / 3; if norm(dx) tol break; end else % 拒绝更新, 增大阻尼 lambda lambda * 2; end end end逻辑说明这是一个最简LM实现没有做雅可比矩阵的数值梯度而是由每个因子自己提供解析雅可比块然后组装成大矩阵。assembleWeightMatrix把每个因子的sqrtInfo按块放到对角线上保证残差和协方差一一对应。参数说明lambda0一般设成1e-4或1e-3迭代后期如果频繁拒绝它会被自动放大。tol设成1e-6是个稳健值太大会提前终止在误差较大处太小则在接近零空间时反复震荡。J的组装方式直接影响H的稀疏结构下一章会用稀疏矩阵重构这一步现在先用稠密矩阵把流程跑通。3.2 MATLAB实现求解器的10个核心函数一个能实际跑数据的因子图求解器通常由10个核心函数组成。我按依赖顺序列出来括号里是每个函数的关键职责addPriorFactor(fg, varId, obs, cov)构造信息矩阵残差为sqrtInfo * (obs - state)。addBetweenFactor(fg, varId1, varId2, relPose, cov)相邻状态间的相对约束残差为relPose - (x2 - x1)。addGPSFactor(fg, varId, positionObs, cov)绝对位置观测残差带一个可选的坐标系变换。computeResidualAndJacobian(fg)遍历所有因子拼接残差向量和雅可比矩阵。computeFactorResidual(fg, factorIdx)单个因子的残差支持数值检验。computeFactorJacobian(fg, factorIdx)解析雅可比块每个因子一行函数方便单测。assembleWeightMatrix(fg)按因子顺序构造稀疏块对角权重矩阵。buildSystem(fg)组合残差、雅可比、权重产出H和b。lmStep(H, b, x, lambda)阻尼增量求解。marginalizeVariable(fg, varId)边缘化旧变量生成一个连接剩余变量的稠密因子。其中最关键的是computeFactorJacobian。比如运动因子x2 - x1 rel残差对x1的雅可比是-sqrtInfo对x2是sqrtInfo先验因子只有单个变量所以它的雅可比就是一个块。写完之后你务必用数值微分验证一遍(f(xeps) - f(x-eps)) / 2eps和解析值之间的误差应该低于1e-6这是矩阵维数错误最有效的防线。3.3 参数怎么设迭代次数、收敛阈值、步长阻尼在MATLAB里跑因子图优化新手最爱用固定迭代次数结束循环。我一般不这么做因为不同传感器组合的收敛速度差异很大纯IMU预积分收敛慢可能要30次迭代有GPS绝对观测时5次迭代就能到1e-6。强迫症不如设一个较大的maxIter50让LM自己提前退出这样既保证精度又不浪费算力。阻尼lambda的增长/缩减因子也有讲究。常见做法是接受时除以3拒绝时乘2这样能让λ快速跳到合适的区间。如果你发现优化后残差平方和比EKF滤波的还大优先怀疑λ初值设成了1这一步会退化成梯度下降导致收敛巨慢。另一个容易翻车的参数是tol——它判断的是增量范数不是代价函数变化。遇到状态尺度差异大角度量级0.1位移量级100增量范数可能被大尺度状态主导这时要把位移、角度分别归一化再判断收敛。4. 因子图与EKF融合固定滞后平滑与增量更新4.1 为什么纯因子图不能完全替代EKF因子图是批处理模型每来一个新观测就把全图重新线性化求解。这对离线SLAM没问题但在实时组合导航里全图规模无限增长每次求解的延迟会从几毫秒涨到几百毫秒。EKF的优势在于它只维护当前状态和协方差计算量恒定。工程上最常见的做法不是二选一而是让因子图做固定滞后平滑只保留最近N个时刻的变量更早期的变量通过边缘化折叠成先验因子。这样一来窗口内依然保留因子图的批量优化能力窗口外的信息不会被遗忘计算量被一条硬边界卡住。固定滞后平滑还有另一个好处它天然支持“后悔药”。在EKF里如果某次观测误匹配如GPS野值修正会永远留在协方差里因子图窗口内则可以把错误因子的残差权重直接降为零或删除然后重新优化一遍窗口状态能立即纠正回来。4.2 用因子图做固定滞后平滑的MATLAB实现固定滞后平滑的关键函数是边缘化marginalization。假设窗口长度N10新因子要添加第11个状态时需要把第1个状态从图里“抽掉”同时产生一个稠密先验因子连接所有与第1个状态相连的剩余变量。function [newPriorFactor, keepVarIds] marginalizeVariable(fg, varId, keepVars) % 边缘化指定变量, 返回一个稠密先验因子 % 找出所有与该变量相连的因子 connectedFactors findConnected(fg, varId); % 收集涉及的变量集合(除去被边缘化的变量) keepVarIds unique([connectedFactors.varIds]); keepVarIds(keepVarIds varId) []; % 构建局部信息矩阵: H J^T W J, 只包含相关因子 H buildLocalSystem(fg, keepVarIds, varId); b buildLocalVector(fg, keepVarIds, varId); % 舒尔补: 得到关于剩余变量的先验信息 H_cc H(keepIdx, keepIdx); % 剩余变量块 H_cm H(keepIdx, edgeIdx); % 交叉块 H_mm H(edgeIdx, edgeIdx); % 被边缘化变量块 H_prior H_cc - H_cm * (H_mm \ H_cm); % 转成先验因子 newPriorFactor.sqrtInfo chol(H_prior, upper); newPriorFactor.obs ...; % 来自局部解算出的最优状态 end这段代码里我用了舒尔补做边缘化。数学上是把联合高斯分布中对x_m的协方差投影到剩余变量上得到的H_prior是一个稠密对称正定矩阵。这个新因子不再是简单的一次或二次约束它是“被边缘化状态的历史信息总和”所以它的sqrtInfo可以直接作为先验因子的平方根信息矩阵。参数说明keepVars要包含所有与新先验因子相连的变量否则信息会丢失。窗口长度N的选取依赖传感器频率和算力预算常见做法是N在50到200之间如果帧率是10HzN100代表窗口10秒足够覆盖GPS遮挡的短时盲区。注意MATLAB里H_prior求逆前的条件数检查如果H_mm接近奇异边缘化会把数值噪声放大这时应该给H_mm对角线加一个很小的正则项1e-9*eye(m)再求逆。4.3 与EKF融合时的信息交互状态、协方差、时间对齐实现融合时不能把因子图的结果直接塞进EKF的预测步当测量用。常见做法是因子图维护的窗口状态作为“参考轨迹”EKF在相邻两个因子图更新之间做高频惯性递推每到一个因子图更新时刻EKF当前状态与因子图窗口内最新状态之差作为“虚拟观测”更新进来。这样高频噪声由EKF吃掉低频漂移由因子图纠正。时间对齐是这里最容易出问题的地方。IMU和相机时间戳必须在加入因子图前完成同步否则因子图的运动因子会因为时间偏置产生系统性残差。做法是在构建因子图前把所有观测时间戳统一到GPS/UTC时间轴上并对IMU预积分做时间插值。用MATLAB的retime处理时间表时注意插值方法应该用线性插值不要用末尾填充否则会造成几分钟级别的跳变。5. 因子图构建与求解避坑5个常见的玄学问题5.1 现象雅可比矩阵维数对不上一迭代就报错我见过最多的报错是“Matrix dimensions must agree”。原因是因子图变量个数和因子内部varIds不一致比如运动因子声明连接变量[1,2]但计算残差时你写的是x2 - x1而实际上computeResidual里按varIds(1)取了状态雅可比块顺序写反。原因变量节点的顺序和因子内部记录的变量id没有强制校验。解决在addFactor时校验varIds的每个id都在variables里存在在computeFactorJacobian里用一个临时map从id映射到变量在全局向量中的索引。另外给每个因子写一个独立的数值雅可比检查函数跑通后再组装。5.2 现象优化后轨迹漂移比EKF还大按理说因子图是全局优化不该比EKF差。但如果你发现结果更飘八成是运动因子的协方差被设小了。运动模型本身有不确定性你把cov填成1e-6等于强迫机器人的相邻位移严格等于里程计读数因子图为了迎合这个强约束会把其他观测全部扭曲掉。解决把运动协方差设成和真实传感器噪声同级比如编码器测距噪声0.05米协方差就填0.05^2。如果多传感器融合初始协方差可以先用启发式值跑一遍再根据残差分布反向调整。记住因子图是“按权重妥协”谁的信息矩阵大谁说了算。5.3 现象加了新因子后旧状态被改动固定滞后平滑里的边缘化应把旧信息折叠成先验但如果你在窗口里既保留了旧变量又添加了连接旧变量和新变量的因子优化时旧变量当然会被带动。另一个常见原因是fixed标记没设对比如你先固定了x1后来添加一个因子同时连接x1和x2x1被固定后x2依然可能因x1的锚定而合理变化但如果你把x1设成不固定整个图会整体飘移。解决明确哪些变量是“窗口外已边缘化”的边缘化后从变量列表里删掉原变量新增的稠密先验因子只连接剩余变量。别让旧变量继续留在图里。5.4 现象信息矩阵奇异求解器崩溃奇异基本出自两种没有任何先验或固定变量或者两个因子对同一组变量的约束完全线性相关例如两个GPS因子在同一位置给出相同观测。用力矩扳手都拧不动的时候矩阵也会提示你。解决至少固定一个变量对重复约束用diag(jitter)给H的对角线叠加一个极小正则项值取1e-9只破坏奇异性不破坏精度。另外初次建图时先不要加闭环因子等普通约束跑通再加。5.5 现象MATLAB循环太慢仿真跑不动因子图求解的根本瓶颈是大矩阵乘和求逆纯MATLAB循环组装雅可比矩阵会让你怀疑人生。我实际调优时做了三件事把变量状态改为列向量存储避免矩阵拼接的二次拷贝预分配J和W的空间而不是每迭代一次就[J; jacobian]拼接对批次因子用sparse构造稀疏矩阵再参与求解而不是用稠密eye。解决把computeResidualAndJacobian里对因子遍历的部分写成生成行索引、列索引、值三个数组最后调用一次sparse。这个改动在N200的状态空间里能快一个量级。6. 最后一步用稀疏Cholesky验证Hessian结构再谈落地的取舍6.1 为什么求解器里要用稀疏矩阵因子图的信息矩阵H天然具有块稀疏结构每个因子只连接少数变量所以H的非零块数量远小于变量数的平方。用稠密矩阵求解N500维的状态H是500x500存储量不算大但求解复杂度是O(n^3)达到125e6次运算而稀疏Cholesky只对非零块做分解复杂度降到接近O(n * 有效连接数)。这也是gtsam和g2o能跑大规模实时问题的底层原因。用MATLAB验证Hessian结构的脚本很直观% 给fg添加完所有因子后, 构建H [r, J] computeResidualAndJacobian(fg); W assembleWeightMatrix(fg); H J * W * J; % 画稀疏性图像 figure; spy(H); title(Factor Graph Hessian Sparsity Pattern); xlabel(Column Index); ylabel(Row Index); % 检查是否对称正定 assert(norm(H - H, fro) 1e-10); eigvals eig(H); fprintf(Smallest eigval: %.3e\n, min(eigvals));运行后你会看到H的非零块集中在一条带状区域内狭长对角带就是连续运动因子产生的离带状区域较远的稀疏散点通常是回环或绝对位置观测造成的。这个图像能帮你直接判断图的拓扑合理性——如果带状区域外没有连接说明你的观测因子没有跨时间约束固定滞后平滑实际上退化成了一堆局部约束。6.2 一个验证脚本的骨架验证因子图求解器是否写对我最常用的办法是拿一段真实数据生成一个小图人为把观测噪声设为零把初值故意设偏再跑求解器。如果实现正确最终残差应接近零状态恢复精确值。加上数值雅可比对比、H对称性检查、LM迭代代价单调性检查全部通过后基本可以放心用到下一阶段。6.3 我现在的选择因子图建模 EKF兜底我在自己的项目里最终没有用纯因子图替代EKF而是采用“因子图做窗口平滑 EKF做高频递推”的双层结构。因子图负责每200ms重新优化一次窗口修正低频漂移和提供全局约束EKF则以100Hz频率在两次优化之间递推IMU和编码器数据保持控制环路所需的实时性。这个架构里因子图的定量置信度来自Hessian矩阵的逆对角线我会定期检查它的条件数如果发现条件数在几个周期内持续上升就能提前预警传感器退化而不是等到轨迹飞了才去找原因。在MATLAB里跑通这套链路相比C实现的好处是你能随时断点检查每个雅可比块和残差坏处是性能上限低。我习惯先把算法验证逻辑写清楚再逐函数翻译成C。你现在拿到的这份代码包我建议你先跑通固定滞后平滑的最小例子然后把你的IMU/GPS数据喂进去观察残差分布和协方差变化。发现某个因子的残差均值明显非零说明模型偏差存在——这才是因子图给你的最大价值它能让你把传感器误差一件件拆出来而不是像EKF那样黑匣子一盖只知道“滤波发散了”。按照上面的步骤走一遍你大概率会在第2个数据集上找到属于自己的那批参数。希望帮到你。本文还有配套的精品资源点击获取