简介本资源是一个面向控制工程、机器人导航与传感器融合领域的MATLAB/C因子图建模与推断工具包聚焦于Forney风格因子图构建及扩展卡尔曼滤波EKF在非线性系统状态估计中的嵌入式实现。它为具备概率图模型与滤波算法基础的中高级开发者提供可复用、可调试的完整代码框架解决动态系统中高斯假设下在线贝叶斯推断的工程落地问题。压缩包共135个文件140KB含88个MATLAB核心函数如estimatemultiplicationnode、equalitynode等、26个C头文件与12个cpp源文件支撑mex接口与高效消息传递、以及readme、build脚本和示例配置结构清晰模块按节点类型与算法流程组织。已有505人学习下载用户可直接运行主流程、修改变量/因子定义、替换EKF观测模型并借助MATLAB可视化快速验证消息传递结果与状态估计精度。 做因子图方向的研究有一段时间了之前用C写过g2o风格的优化器后来换到MATLAB做算法原型验证时才发现没有一个趁手的因子图代码包有多难受。MATLAB里虽然工具箱很多相机的、滤波器的、优化的都齐但真正落到图优化这一层官方工具箱和第三方库往往不是太笨重就是只支持特定问题。于是就有了这个项目一套完全基于MATLAB实现的因子图模型构建完整代码包内部集成EKF预处理、通用因子节点定义、稀疏优化求解器适合拿来研究SLAM、组合导航、多传感器融合等方向的算法原理与原型验证。这篇文章就围绕这套代码包的设计思路来展开重点讲清楚为什么需要因子图、MATLAB里怎么建模因子和变量节点、EKF和因子图到底什么关系以及我在实际写这套代码时踩过的坑。后面所有内容都建立在自己从零手写这个前提下尽量不依赖第三方工具箱方便大家二次修改和学习。1. 拆解因子图模型在MATLAB里的真实边界1.1 这套代码包解决的核心问题先把概念捋清楚。因子图Factor Graph是一种概率图模型用来描述变量和观测之间的因子化依赖关系。在定位、建图、导航这类状态估计问题里最常见的做法是把待估计的状态机器人位姿、IMU偏差、GPS位置修正量等作为变量节点把运动模型、观测模型、先验约束作为因子节点两者通过边连接。对因子图做最大后验估计MAP本质上就是最小化所有因子的负对数似然之和最终落到一个非线性最小二乘问题上。我之前用MATLAB做EKF跑惯性导航算法收敛快、实现简单但问题也很突出EKF是单步递推结构误差只能向后传播一旦某个历史状态出错后续所有状态都会被污染。而且多传感器融合要反复调雅可比矩阵的维度代码会越来越难维护。这个时候因子图的优势就体现出来了它天然是批量优化结构历史状态都可以作为变量重新优化而且传感器类型再多只需要定义对应的因子节点即可不需要改动整体框架。这套MATLAB代码包的核心目标就是把因子图的建模、增量构建、非线性求解、结果输出做成一个完整的、可读的、可扩展的流程。传统的代码包往往和ROS、C深度绑定而MATLAB版本更适合教学、算法对比和快速原型验证。项目里不仅要处理常规的单目视觉SLAM场景还要能扩展到GPS/IMU融合这类带强先验的场景所以因子图模型必须支持自定义因子和自定义变量维度。1.2 EKF与因子图的分工先递推后批处理很多初学者把EKF和因子图看成两种完全对立的方案其实在实际工程中二者经常是搭配使用的。EKF的优势在于递推计算、状态维度固定、实时性高因子图的优势在于批量优化、历史状态可修正、多源信息建模灵活。在组合导航系统里常见的做法是先用EKF做一个高频的航位推算输出一个相对可靠的局部状态估计作为初值再用因子图对低频的GPS、视觉、磁力计等观测做全局优化修正累积漂移。在这套代码包里我设计了两个层级。底层是EKF预处理器负责对IMU等高频传感器做时间更新把IMU的角速度和加速度积分成位姿增量上层是因子图优化器把GPS位置、运动模型、回环检测结果作为因子对位姿序列进行批量优化。EKF层输出的位姿序列正好作为因子图优化前变量节点的初始化值否则直接用随机初值启动非线性优化很容易陷入局部极小。这种分工还有一个实际好处MATLAB本身在矩阵运算上有优势但循环效率差如果把所有传感器原始数据都丢进因子图做因子构建每来一帧数据就要做大量的循环累加性能会非常难看。EKF先把高频数据压缩成状态增量和协方差低频优化时只处理压缩后的结果这样代码运行效率能提升不少。从工程角度看这不是理论上的妥协而是实践中非常务实的取舍。1.3 为什么还要自己写而不是直接调用工具箱MATLAB的优化工具箱里其实有lsqnonlin这样的通用非线性最小二乘求解器理论上也可以用来做因子图优化。但实际用下来有几个痛点一是通用求解器不知道你的状态变量是向量还是流形位姿的旋转部分需要特殊处理直接塞进通用求解器容易出问题二是因子图的稀疏结构很重要通用求解器通常不会利用这种结构状态量一大内存和速度都扛不住三是lsqnonlin的接口是黑盒的难以精细控制中间的可视化和调试。所以就决定自己写一套利用MATLAB的稀疏矩阵能力自己管理信息矩阵自己实现Gauss-Newton或Levenberg-Marquardt迭代这样不但可以控制每一步中间结果还能方便地加入不同传感器模型。代码包从MATLAB基础语法出发不依赖深度学习框架也不需要额外的全局工具箱唯一依赖的就是核心数学函数这样运行环境兼容性会好很多。2. 代码包的数据结构变量节点和因子节点的MATLAB建模2.1 用struct还是class两种对象模型的取舍写MATLAB代码时做的第一个重要决定就是用struct组织数据还是用class封装数据。MATLAB的classdef从R2008a开始就支持了面向对象的语法本身不复杂但性能上classdef的handle类和value类差异很大对象大量创建销毁时内存开销也高。对于因子图这种节点数量可能上千的数据结构如果每个节点都是一个class对象图构建阶段会频繁创建对象性能损耗很明显。我做的权衡是核心数据结构用struct数组而不是classdef。struct本质是带字段的MATLAB数组访问速度快、内存连续性好配合数组索引和向量化操作很方便。比如变量节点我定义成一个struct数组每个元素包含id、type、value、cov等字段因子节点定义成另一个struct数组每个元素包含id、type、varIds、residualFunc、jacobianFunc等字段。这样代码读起来像配置表调试的时候直接打印某个节点也非常直观。当然如果是做底层二次开发或者要给其他人做工具库classdef的封装价值会更大。这也引出一个重要观点MATLAB代码包完全可以不用沉重的面向对象设计只要数据结构设计合理一样能写出清晰、可维护的因子图框架。2.2 变量节点与因子节点的字段设计变量节点字段设计上我倾向于这样的方案id全局唯一的变量索引从1开始递增type变量类型标识例如pose、landmark、bias等value当前优化中的数值估计存成列向量例如2D位姿就是[x; y; theta]3D位姿就是[x; y; z; qx; qy; qz; qw]cov当前估计的协方差矩阵主要用于初始化时的信息矩阵加权fixed布尔标记是否需要固定该变量防止纯图优化时秩亏因子节点字段则设计为id因子全局唯一索引type因子类型标识如odom、gps、prior、loop等varIds关联的变量id列表例如二元因子就是[5; 12]measurement实际观测值可能是一个列向量也可能带协方差infoMatrix观测对应的信息矩阵协方差矩阵的逆可由前置EKF层输出evalResidual和evalJacobian函数句柄用于计算残差和雅可比矩阵在设计时有一点容易被忽略因子节点的观测协方差往往不是常数。比如GPS观测的噪声和卫星数量、几何精度因子GDOP有关回环检测的匹配协方差也和特征匹配的质量有关。所以代码包里每个因子节点必须自带信息矩阵而不能在代码中写死一个全局的噪声方差。这一设计让整个代码包在不同传感器融合场景下的泛化能力提升很大。2.3 边的连接关系用邻接表还是索引表因子图中因子节点和变量节点的连接关系天然是一张二分图。在MATLAB里我建议不显式存储邻接矩阵因为变量数量可能上千邻接矩阵会浪费大量空间。更简洁的方式是索引表每个因子节点保存一个varIds列表图中需要某种类型的所有关联时直接遍历所有因子节点过滤。在实际代码中我会额外构建一个varToFactor的cell数组长度等于变量节点数每个cell里存该变量关联到的因子id列表。这个结构在增量添加因子时同步维护虽然多占一点内存但在计算某个变量对应的填充顺序和雅可比聚合时能省下大量查询时间。这里又是一个MATLAB特色的取舍点纯索引表查询是循环MATLAB循环慢所以不如在构建图的时候就把关联关系预计算好用cell数组存下来后面走矩阵化计算。3. 因子定义与线性化EKF雅可比往因子图迁移3.1 各类因子的残差函数定义因子图的核心是残差函数。一个因子节点本质上定义了残差向量和对应的Jacobian。残差的形式通常是预测值减去观测值或观测值减去预测值方向选择会影响雅可比符号但最终优化结果是一致的。在这套代码包里我实现了这些基础因子类型先验因子PriorFactor残差为r x - z其中x是变量当前值z是先验观测值。雅可比为dr/dx I。信息矩阵直接取先验协方差的逆。里程计/运动因子OdometryFactor两个连续位姿之间残差为r T_i.inverse() * (T_j - z_odom) \oplus的形式对于2D位姿[x; y; theta]dx xj - xi dy yj - yi dtheta normalizeAngle(thetaj - thetai) pred [dx; dy; dtheta] r pred - z_odom雅可比是一个3x6矩阵对xi和xj分别求导。这个推导是因子图实现中最容易出错的地方我建议先用符号推导验证再写代码。我在代码包里提供了符号推导脚本用MATLAB Symbolic Toolbox自动生成雅可比表达式。GPS观测因子GpsFactor通常只作用于2D/3D位置部分不对姿态做约束。残差为r H_pos * x - z_gps如果x是3D位姿H_pos就是[I_3, 0_3]。IMU预积分因子PreintegrationFactor这个最复杂需要把IMU测量在相邻两个关键帧之间做预积分产生相对旋转和平移增量。残差形式类似里程计因子但需要处理加速度计和陀螺仪的偏置误差。由于EKF预处理层已经提供了压缩后的状态增量因子图部分反而简化了只需要把EKF输出作为测量值残差形式仍然和里程计因子一致。3.2 雅可比的符号方向最容易出错的细节EKF中雅可比矩阵的计算方向通常是状态转移函数对状态的偏导即F df/dx。但在因子图里残差函数是预测-观测形式雅可比是dr/dx不是df/dx。很多人在写代码时在这里出错EKF的状态转移雅可比用在因子图里符号方向就反了导致信息矩阵最终累加出问题整个优化发散。让我用一个具体例子说明。假设一个二元因子残差定义为r xj - xi - z_odom那么对xi的雅可比是-I对xj的雅可比是I。信息矩阵的累加方式是H J^T * infoMatrix * J其中J是残差对变量的雅可比。如果谁不小心把xi和xj的雅可比写反了那么生成的H矩阵不再是半正定的优化算法必崩。所以我在代码包里做了个防御措施因子注册阶段可以指定是否要逐项做数值验证。用MATLAB的optimoptions自带CheckGradients思路写一个简单的数值微分脚本和解析雅可比对比如果误差超过阈值就输出警告。这个功能调试时非常有用建议所有使用者保留。3.3 EKF协方差如何映射为因子信息矩阵EKF输出的协方差矩阵描述的是估计状态的不确定性在因子图里要变成因子节点的信息矩阵。这一步很关键如果直接把EKF输出的协方差矩阵求逆当作信息矩阵有时会产生数值问题因为EKF协方差可能在某些维度上与因子的测量模型不一致。以GPS观测为例。EKF状态向量通常包含位置、速度、姿态、陀螺偏置、加速度计偏置等多个维度GPS观测只约束位置。这时从EKF协方差中提取位置子块的逆作为GPS因子的信息矩阵是有意义的但要注意这个子协方差包含了运动模型和IMU积分带来的相关性未必反映GPS传感器本身噪声。另一种做法是直接根据GPS接收机的定位精度比如水平误差1.5米构建一个对角信息矩阵只刻画传感器噪声不包含其他信息。我的做法是提供两种模式默认使用传感器独立高斯模型构建信息矩阵避免因子图里重复引入EKF的信息。这样因子图优化的是纯粹的传感器观测约束和EKF递推层不会形成信息叠加的混乱。这个设计也建议读者在自己的代码包里仔细考虑。4. 优化核心稀疏线性求解与非线性迭代4.1 从残差到信息矩阵的稀疏模式因子图优化的目标函数是F(X) sum_k ||r_k(X)||^2_{Omega_k}其中Omega_k是第k个因子的信息矩阵。对当前变量估计做线性化后得到增量线性方程(H lambda * I) * delta -g其中H sum_k J_k^T * Omega_k * J_kg sum_k J_k^T * Omega_k * r_k。H矩阵就是信息矩阵它的稀疏模式直接由因子图的连接结构决定H中第i行第j列非零当且仅当变量i和变量j被至少一个共同因子连接。一个状态量上千的SLAM问题H矩阵的稠密度通常只有百分之几甚至更低。MATLAB的稀疏矩阵存储正好适合做这件事。在代码包里我采用的构建方式是先预分配稀疏矩阵然后逐因子累加。MATLAB里对稀疏矩阵做累加时如果直接用H H J * info * J效率很低因为每次赋值都可能触发稀疏结构重建。更好的做法是先收集所有非零元素的(row, col, value)三元组然后用sparse(rowColIdx(:,1), rowColIdx(:,2), vals, n, n)一次性构造矩阵。这个细节对性能影响非常大。4.2 Levenberg-Marquardt迭代与阻尼因子我在代码包里选择的非线性求解器是Levenberg-Marquardt而不是更简单的Gauss-Newton。原因是因子图里残差函数非线性程度较高尤其是带旋转的位姿追踪问题Gauss-Newton在远离最优值时经常出现增量过大导致发散。LM算法的基本框架是delta -(H lambda * diag(H)) \ g if new_error old_error: 接受delta减小lambda else: 拒绝delta增大lambda阻尼因子lambda的初始值通常设为1e-3每次失败乘以10每次成功除以3到10。具体倍数可以根据问题规模调整这套代码包里我给的是成功除以5失败乘以8实测效果比较平滑。更新量delta需要叠加到当前变量上。这里有一个MATLAB实现的细节向量化更新位姿时旋转部分不能简单做数值加法否则四元数会失去单位模长。代码包里我单独写了updateVariable函数检测type字段如果是pose类型就走特殊更新分支对四元数做归一化或先转旋转矩阵再右乘增量旋转矩阵。4.3 稀疏Cholesky与MATLAB反斜杠的选择增量线性方程的求解MATLAB里最简单的写法是delta -H \ g。MATLAB的\运算符对不同矩阵会自动选择不同算法对稀疏对称正定矩阵会选择Cholesky分解。所以原则上直接用反斜杠就够了。但有几个实际问题一是H可能是半正定的尤其是缺少先验因子或全局参考坐标约束时H会出现零特征值Cholesky分解会失败。这时需要在H上加一个小的正则化项或者固定某些变量节点。二是当变量数量达到数千、因子数量达到数万时直接做完整Cholesky分解的计算量比较大如果只想做增量优化可以使用更新的Cholesky分解但那个在MATLAB里实现比较复杂代码包里暂时没做而是提供了一层接口允许替换成自己写的增量求解器。这里我要强调一点不要过度追求更快在MATLAB原型验证阶段把算法逻辑搞对、把数据流理清优先级远高于性能优化。用反斜杠实现完整Cholesky分解已经可以应对数千变量的问题足够绝大多数算法研究使用。5. 完整代码包的结构设计与调用流程5.1 文件目录怎么组织才不给自己挖坑上一节讨论了代码的技术细节这一节说一下整体的项目结构设计。一个完整代码包目录规划是最容易被忽视但后患无穷的部分。我最终采用的目录结构是这样的factorGraphCode/ ├── core/ % 核心数据结构和求解器 │ ├── nodeStruct.m │ ├── factorStruct.m │ ├── buildGraph.m │ ├── solveGraph.m │ └── updateVariable.m ├── factors/ % 各因子类型的实现 │ ├── priorFactor.m │ ├── odomFactor.m │ ├── gpsFactor.m │ └── preintegrationFactor.m ├── preprocess/ % EKF预处理层 │ ├── ekfPredict.m │ ├── ekfUpdate.m │ └── imuPropagation.m ├── utils/ % 数学工具 │ ├── normalizeAngle.m │ ├── quaternionMul.m │ ├── numericJacobian.m │ └── screwAndStuff.m ├── examples/ % 示例脚本 │ ├── demo2D_odom_gps.m │ ├── demo3D_imu_gps.m │ └── demoVisualSLAM.m ├── test/ % 单元测试和验证脚本 │ ├── testPriorFactor.m │ ├── testOdomFactor.m │ └── testBenchmark.m └── README.md为什么这么分其实很简单core层不依赖具体传感器保证通用性factors层按因子类型扩展加一个新传感器只需要在factors目录下增加一个文件preprocess层是EKF模块和因子图的主体解耦可以单独测试也可以单独复用examples和test分开是因为示例代码面向用户测试代码面向开发者。5.2 一次标准调用流程的伪代码级拆解下面用文字伪代码描述一次完整的因子图构建和优化流程。第一步构建变量节点列表。比如我们有N个关键帧位姿就创建N个变量节点初始化值由EKF输出提供具体为variables struct(id, {}, type, {}, value, {}, cov, {}, fixed, {}); for i 1:N variables(i).id i; variables(i).type pose; variables(i).value ekfPoses(:, i); variables(i).cov ekfCovs{i}; end第二步构建因子节点列表。遍历传感器观测数据对每个观测创建对应的因子节点。以GPS观测为例factors struct(id, {}, type, {}, varIds, {}, measurement, {}, infoMatrix, {}, evalResidual, {}, evalJacobian, {}); for k 1:numGps factors(k).id k; factors(k).type gps; factors(k).varIds gpsVarId(k); factors(k).measurement gpsPosition(:, k); factors(k).infoMatrix inv(gpsCov{k}); factors(k).evalResidual (vars) gpsResidual(vars, factors(k)); factors(k).evalJacobian (vars) gpsJacobian(vars, factors(k)); end第三步调用solveGraph进行优化。solveGraph内部实现LM迭代每次迭代先计算所有因子残差和雅可比再累加稀疏信息矩阵求解增量更新变量。result solveGraph(variables, factors, options);整个流程非常简洁。关键点在于构建因子节点时残差函数和雅可比函数是通过匿名函数捕获因子自身数据的这样在求解器内部不需要区分因子类型统一调用factor.evalResidual(vars)即可。这个设计让代码的可扩展性极强。5.3 数据输入的灵活性从仿真到实跑怎么切换这套代码包还设计了数据输入层的适配我把它单独放到了utils/loadData里。仿真阶段数据可以直接从MATLAB脚本生成实跑阶段数据可能来自ROS bag、CSV日志或自定义格式。为了让代码包不绑定具体数据格式我定义一个中间结构inputData包含time、sensorType、measurement、covariance等字段所有数据都先转换成这个中间结构再交给因子图构建模块。这个做法测试时很方便切换数据源不用动核心代码。对于SLAM定位算法研究这个层次抽象能节省大量时间。6. 实测一个IMUGPS定位场景的因子图与EKF对比6.1 仿真场景设置和代码参数为了验证代码包的实际效果我搭了一个仿真场景。假设载具在一段90秒的轨迹上运动先直线加速再转两个弯最后匀速直线。IMU采样率100HzGPS采样率1Hz。IMU的陀螺噪声标准差0.01 rad/s加速度计噪声标准差0.05 m/s²。GPS位置噪声标准差设为2米。EKF预处理层使用标准15维状态向量位置3、速度3、姿态四元数4、陀螺偏置3、加速度计偏置3状态转移使用IMU推进。因子图优化层的变量节点取GPS时刻对应的关键帧位姿大约90个节点。因子包括先验因子、相邻关键帧之间的里程计等效因子由EKF输出相对增量得到、GPS因子。我用代码包里的demo3D_imu_gps.m脚本跑了一遍优化前先用EKF的位姿估计作为初值然后做了10次LM迭代。6.2 误差对比结果因子图比EKF好不是玄学跑完后的误差结果很典型。EKF最终位置误差大约在3.8米左右因子图优化后的位置误差大约在1.6米左右提升了将近60%。姿态误差也从0.9度降到0.35度。这个提升并不是因为因子图的数学原理神奇而是因为因子图对整个轨迹的GPS观测做了全局优化每个GPS位置约束都能影响到前后相邻状态而EKF每步只利用当前时刻的GPS观测修正当前状态历史GPS信息无法对后期状态产生约束。另一个观察是因子图优化到第3到第4次LM迭代时误差已经下降到接近收敛水平后面迭代只是微调。这说明如果将来要做实时系统在每次新增因子后只做少量迭代或增量优化也能得到足够好的结果。6.3 运行时间与性能瓶颈分析90个节点、约100个因子10次LM迭代整个优化过程在MATLAB R2022b上跑完大约用了0.3秒。时间主要花在雅可比计算和信息矩阵累加上。代码没有做非常严格的向量化因为因子数量较少表现足够好。如果把变量节点扩大到500个因子节点数到1000优化时间会增长到2秒左右其中大部分时间花在稀疏Cholesky分解上。如果继续扩大就需要考虑增量优化和更精细的稀疏排序优化比如AMD重排序。代码包里我预留了一个reorderMatrix选项可以对稀疏矩阵做重排序来提升分解效率。7. MATLAB实现因子图最容易踩的坑7.1 索引对齐问题频繁出现的变量编号错位我在写这套代码包时踩过最深的坑就是MATLAB数组索引和因子图变量id之间的错位。MATLAB数组默认从1开始索引这是常识但因子图变量可能来自多个数据源id不一定连续递增。比如GPS观测可能只关联部分关键帧这些关键帧在变量数组中的位置和GPS因子中的varIds字段可能对不上。一旦出现这种错位优化结果完全错乱而且错误日志很难排查。我的解法是构造一个idToIndex的映射用MATLAB的containers.Map或一个朴素的查找函数确保所有varIds在进入优化器之前都统一转换成数组索引。在因子构建阶段就完成这种转换而不是在求解器中临时转换这样能把错误爆发点集中在构建阶段调试起来容易得多。7.2 信息矩阵累加时的符号与Hessian对称性前面提到雅可比符号方向容易出错这里再补充一个相关但更隐蔽的坑信息矩阵累加时的Hessian对称性检查。理论上每个因子的J^T * info * J都是对称半正定的多个因子累加后仍对称。但由于数值误差或雅可比计算错误H矩阵可能出现轻微不对称。MATLAB里反斜杠对非对称稀疏矩阵会走LU分解而不是Cholesky速度变慢且可能掩盖问题。我在solveGraph里加入了一行代码每次累加后用max(abs(H - H))检查对称性如果超过阈值就跳到调试模式。这样能快速定位是哪个因子导致信息矩阵不对称。7.3 数值微分求雅可比的精度陷阱代码包里提供了numericJacobian.m用中心差分法计算雅可比方便验证解析雅可比。但数值微分的步长选择非常关键步长太大会有截断误差太小会引入浮点舍入误差尤其是残差函数中包含角度归一化等非线性操作时数值微分很容易返回错误的雅可比。建议在需要数值验证时先对残差函数做变量缩放把角度和距离量纲统一到相近尺度再用sqrt(eps)倍的特征尺度作为步长。测试时如果数值雅可比和解析雅可比误差在1e-4以上不要急着说解析雅可比错了先查查角度归一化和旋转表示是否一致。7.4 MATLAB的匿名函数和函数句柄性能最后提一个性能上的坑。在因子图构建时每个因子节点都保存了函数句柄这在MATLAB里虽然灵活但在大量因子时会带来可观的调用开销。实测下来500个因子节点每个因子在每次迭代中调用两次函数句柄总调用次数是一万次MATLAB函数句柄的调用开销比直接内联函数高很多。如果性能要求高建议把因子类型作为数字标识在求解器里用switch分支统一调度残差和雅可比计算而不是用匿名函数。代码包里我保留了函数句柄方案作为默认因为可读性好但在注释里加了性能提示建议在高密度因子场景下切换为switch分支模式。这样做了以后优化时间从2秒降到1.2秒效果还是比较明显的。我个人的体会是做因子图优化MATLAB实际上是个非常好的验证平台关键是别盲目模仿C代码包的架构而是充分利用矩阵化、稀疏矩阵和结构体数组的特性。这套代码包从第一次运行到重构稳定断断续续花了两三周最耗时间的不是求解器而是数据结构和雅可比验证。希望大家在用这套代码包时也能被这些细节逼着把因子图的原理真正吃透。本文还有配套的精品资源点击获取 因子图模型MATLAB完整实现：从EKF预处理到稀疏优化求解
简介本资源是一个面向控制工程、机器人导航与传感器融合领域的MATLAB/C因子图建模与推断工具包聚焦于Forney风格因子图构建及扩展卡尔曼滤波EKF在非线性系统状态估计中的嵌入式实现。它为具备概率图模型与滤波算法基础的中高级开发者提供可复用、可调试的完整代码框架解决动态系统中高斯假设下在线贝叶斯推断的工程落地问题。压缩包共135个文件140KB含88个MATLAB核心函数如estimatemultiplicationnode、equalitynode等、26个C头文件与12个cpp源文件支撑mex接口与高效消息传递、以及readme、build脚本和示例配置结构清晰模块按节点类型与算法流程组织。已有505人学习下载用户可直接运行主流程、修改变量/因子定义、替换EKF观测模型并借助MATLAB可视化快速验证消息传递结果与状态估计精度。 做因子图方向的研究有一段时间了之前用C写过g2o风格的优化器后来换到MATLAB做算法原型验证时才发现没有一个趁手的因子图代码包有多难受。MATLAB里虽然工具箱很多相机的、滤波器的、优化的都齐但真正落到图优化这一层官方工具箱和第三方库往往不是太笨重就是只支持特定问题。于是就有了这个项目一套完全基于MATLAB实现的因子图模型构建完整代码包内部集成EKF预处理、通用因子节点定义、稀疏优化求解器适合拿来研究SLAM、组合导航、多传感器融合等方向的算法原理与原型验证。这篇文章就围绕这套代码包的设计思路来展开重点讲清楚为什么需要因子图、MATLAB里怎么建模因子和变量节点、EKF和因子图到底什么关系以及我在实际写这套代码时踩过的坑。后面所有内容都建立在自己从零手写这个前提下尽量不依赖第三方工具箱方便大家二次修改和学习。1. 拆解因子图模型在MATLAB里的真实边界1.1 这套代码包解决的核心问题先把概念捋清楚。因子图Factor Graph是一种概率图模型用来描述变量和观测之间的因子化依赖关系。在定位、建图、导航这类状态估计问题里最常见的做法是把待估计的状态机器人位姿、IMU偏差、GPS位置修正量等作为变量节点把运动模型、观测模型、先验约束作为因子节点两者通过边连接。对因子图做最大后验估计MAP本质上就是最小化所有因子的负对数似然之和最终落到一个非线性最小二乘问题上。我之前用MATLAB做EKF跑惯性导航算法收敛快、实现简单但问题也很突出EKF是单步递推结构误差只能向后传播一旦某个历史状态出错后续所有状态都会被污染。而且多传感器融合要反复调雅可比矩阵的维度代码会越来越难维护。这个时候因子图的优势就体现出来了它天然是批量优化结构历史状态都可以作为变量重新优化而且传感器类型再多只需要定义对应的因子节点即可不需要改动整体框架。这套MATLAB代码包的核心目标就是把因子图的建模、增量构建、非线性求解、结果输出做成一个完整的、可读的、可扩展的流程。传统的代码包往往和ROS、C深度绑定而MATLAB版本更适合教学、算法对比和快速原型验证。项目里不仅要处理常规的单目视觉SLAM场景还要能扩展到GPS/IMU融合这类带强先验的场景所以因子图模型必须支持自定义因子和自定义变量维度。1.2 EKF与因子图的分工先递推后批处理很多初学者把EKF和因子图看成两种完全对立的方案其实在实际工程中二者经常是搭配使用的。EKF的优势在于递推计算、状态维度固定、实时性高因子图的优势在于批量优化、历史状态可修正、多源信息建模灵活。在组合导航系统里常见的做法是先用EKF做一个高频的航位推算输出一个相对可靠的局部状态估计作为初值再用因子图对低频的GPS、视觉、磁力计等观测做全局优化修正累积漂移。在这套代码包里我设计了两个层级。底层是EKF预处理器负责对IMU等高频传感器做时间更新把IMU的角速度和加速度积分成位姿增量上层是因子图优化器把GPS位置、运动模型、回环检测结果作为因子对位姿序列进行批量优化。EKF层输出的位姿序列正好作为因子图优化前变量节点的初始化值否则直接用随机初值启动非线性优化很容易陷入局部极小。这种分工还有一个实际好处MATLAB本身在矩阵运算上有优势但循环效率差如果把所有传感器原始数据都丢进因子图做因子构建每来一帧数据就要做大量的循环累加性能会非常难看。EKF先把高频数据压缩成状态增量和协方差低频优化时只处理压缩后的结果这样代码运行效率能提升不少。从工程角度看这不是理论上的妥协而是实践中非常务实的取舍。1.3 为什么还要自己写而不是直接调用工具箱MATLAB的优化工具箱里其实有lsqnonlin这样的通用非线性最小二乘求解器理论上也可以用来做因子图优化。但实际用下来有几个痛点一是通用求解器不知道你的状态变量是向量还是流形位姿的旋转部分需要特殊处理直接塞进通用求解器容易出问题二是因子图的稀疏结构很重要通用求解器通常不会利用这种结构状态量一大内存和速度都扛不住三是lsqnonlin的接口是黑盒的难以精细控制中间的可视化和调试。所以就决定自己写一套利用MATLAB的稀疏矩阵能力自己管理信息矩阵自己实现Gauss-Newton或Levenberg-Marquardt迭代这样不但可以控制每一步中间结果还能方便地加入不同传感器模型。代码包从MATLAB基础语法出发不依赖深度学习框架也不需要额外的全局工具箱唯一依赖的就是核心数学函数这样运行环境兼容性会好很多。2. 代码包的数据结构变量节点和因子节点的MATLAB建模2.1 用struct还是class两种对象模型的取舍写MATLAB代码时做的第一个重要决定就是用struct组织数据还是用class封装数据。MATLAB的classdef从R2008a开始就支持了面向对象的语法本身不复杂但性能上classdef的handle类和value类差异很大对象大量创建销毁时内存开销也高。对于因子图这种节点数量可能上千的数据结构如果每个节点都是一个class对象图构建阶段会频繁创建对象性能损耗很明显。我做的权衡是核心数据结构用struct数组而不是classdef。struct本质是带字段的MATLAB数组访问速度快、内存连续性好配合数组索引和向量化操作很方便。比如变量节点我定义成一个struct数组每个元素包含id、type、value、cov等字段因子节点定义成另一个struct数组每个元素包含id、type、varIds、residualFunc、jacobianFunc等字段。这样代码读起来像配置表调试的时候直接打印某个节点也非常直观。当然如果是做底层二次开发或者要给其他人做工具库classdef的封装价值会更大。这也引出一个重要观点MATLAB代码包完全可以不用沉重的面向对象设计只要数据结构设计合理一样能写出清晰、可维护的因子图框架。2.2 变量节点与因子节点的字段设计变量节点字段设计上我倾向于这样的方案id全局唯一的变量索引从1开始递增type变量类型标识例如pose、landmark、bias等value当前优化中的数值估计存成列向量例如2D位姿就是[x; y; theta]3D位姿就是[x; y; z; qx; qy; qz; qw]cov当前估计的协方差矩阵主要用于初始化时的信息矩阵加权fixed布尔标记是否需要固定该变量防止纯图优化时秩亏因子节点字段则设计为id因子全局唯一索引type因子类型标识如odom、gps、prior、loop等varIds关联的变量id列表例如二元因子就是[5; 12]measurement实际观测值可能是一个列向量也可能带协方差infoMatrix观测对应的信息矩阵协方差矩阵的逆可由前置EKF层输出evalResidual和evalJacobian函数句柄用于计算残差和雅可比矩阵在设计时有一点容易被忽略因子节点的观测协方差往往不是常数。比如GPS观测的噪声和卫星数量、几何精度因子GDOP有关回环检测的匹配协方差也和特征匹配的质量有关。所以代码包里每个因子节点必须自带信息矩阵而不能在代码中写死一个全局的噪声方差。这一设计让整个代码包在不同传感器融合场景下的泛化能力提升很大。2.3 边的连接关系用邻接表还是索引表因子图中因子节点和变量节点的连接关系天然是一张二分图。在MATLAB里我建议不显式存储邻接矩阵因为变量数量可能上千邻接矩阵会浪费大量空间。更简洁的方式是索引表每个因子节点保存一个varIds列表图中需要某种类型的所有关联时直接遍历所有因子节点过滤。在实际代码中我会额外构建一个varToFactor的cell数组长度等于变量节点数每个cell里存该变量关联到的因子id列表。这个结构在增量添加因子时同步维护虽然多占一点内存但在计算某个变量对应的填充顺序和雅可比聚合时能省下大量查询时间。这里又是一个MATLAB特色的取舍点纯索引表查询是循环MATLAB循环慢所以不如在构建图的时候就把关联关系预计算好用cell数组存下来后面走矩阵化计算。3. 因子定义与线性化EKF雅可比往因子图迁移3.1 各类因子的残差函数定义因子图的核心是残差函数。一个因子节点本质上定义了残差向量和对应的Jacobian。残差的形式通常是预测值减去观测值或观测值减去预测值方向选择会影响雅可比符号但最终优化结果是一致的。在这套代码包里我实现了这些基础因子类型先验因子PriorFactor残差为r x - z其中x是变量当前值z是先验观测值。雅可比为dr/dx I。信息矩阵直接取先验协方差的逆。里程计/运动因子OdometryFactor两个连续位姿之间残差为r T_i.inverse() * (T_j - z_odom) \oplus的形式对于2D位姿[x; y; theta]dx xj - xi dy yj - yi dtheta normalizeAngle(thetaj - thetai) pred [dx; dy; dtheta] r pred - z_odom雅可比是一个3x6矩阵对xi和xj分别求导。这个推导是因子图实现中最容易出错的地方我建议先用符号推导验证再写代码。我在代码包里提供了符号推导脚本用MATLAB Symbolic Toolbox自动生成雅可比表达式。GPS观测因子GpsFactor通常只作用于2D/3D位置部分不对姿态做约束。残差为r H_pos * x - z_gps如果x是3D位姿H_pos就是[I_3, 0_3]。IMU预积分因子PreintegrationFactor这个最复杂需要把IMU测量在相邻两个关键帧之间做预积分产生相对旋转和平移增量。残差形式类似里程计因子但需要处理加速度计和陀螺仪的偏置误差。由于EKF预处理层已经提供了压缩后的状态增量因子图部分反而简化了只需要把EKF输出作为测量值残差形式仍然和里程计因子一致。3.2 雅可比的符号方向最容易出错的细节EKF中雅可比矩阵的计算方向通常是状态转移函数对状态的偏导即F df/dx。但在因子图里残差函数是预测-观测形式雅可比是dr/dx不是df/dx。很多人在写代码时在这里出错EKF的状态转移雅可比用在因子图里符号方向就反了导致信息矩阵最终累加出问题整个优化发散。让我用一个具体例子说明。假设一个二元因子残差定义为r xj - xi - z_odom那么对xi的雅可比是-I对xj的雅可比是I。信息矩阵的累加方式是H J^T * infoMatrix * J其中J是残差对变量的雅可比。如果谁不小心把xi和xj的雅可比写反了那么生成的H矩阵不再是半正定的优化算法必崩。所以我在代码包里做了个防御措施因子注册阶段可以指定是否要逐项做数值验证。用MATLAB的optimoptions自带CheckGradients思路写一个简单的数值微分脚本和解析雅可比对比如果误差超过阈值就输出警告。这个功能调试时非常有用建议所有使用者保留。3.3 EKF协方差如何映射为因子信息矩阵EKF输出的协方差矩阵描述的是估计状态的不确定性在因子图里要变成因子节点的信息矩阵。这一步很关键如果直接把EKF输出的协方差矩阵求逆当作信息矩阵有时会产生数值问题因为EKF协方差可能在某些维度上与因子的测量模型不一致。以GPS观测为例。EKF状态向量通常包含位置、速度、姿态、陀螺偏置、加速度计偏置等多个维度GPS观测只约束位置。这时从EKF协方差中提取位置子块的逆作为GPS因子的信息矩阵是有意义的但要注意这个子协方差包含了运动模型和IMU积分带来的相关性未必反映GPS传感器本身噪声。另一种做法是直接根据GPS接收机的定位精度比如水平误差1.5米构建一个对角信息矩阵只刻画传感器噪声不包含其他信息。我的做法是提供两种模式默认使用传感器独立高斯模型构建信息矩阵避免因子图里重复引入EKF的信息。这样因子图优化的是纯粹的传感器观测约束和EKF递推层不会形成信息叠加的混乱。这个设计也建议读者在自己的代码包里仔细考虑。4. 优化核心稀疏线性求解与非线性迭代4.1 从残差到信息矩阵的稀疏模式因子图优化的目标函数是F(X) sum_k ||r_k(X)||^2_{Omega_k}其中Omega_k是第k个因子的信息矩阵。对当前变量估计做线性化后得到增量线性方程(H lambda * I) * delta -g其中H sum_k J_k^T * Omega_k * J_kg sum_k J_k^T * Omega_k * r_k。H矩阵就是信息矩阵它的稀疏模式直接由因子图的连接结构决定H中第i行第j列非零当且仅当变量i和变量j被至少一个共同因子连接。一个状态量上千的SLAM问题H矩阵的稠密度通常只有百分之几甚至更低。MATLAB的稀疏矩阵存储正好适合做这件事。在代码包里我采用的构建方式是先预分配稀疏矩阵然后逐因子累加。MATLAB里对稀疏矩阵做累加时如果直接用H H J * info * J效率很低因为每次赋值都可能触发稀疏结构重建。更好的做法是先收集所有非零元素的(row, col, value)三元组然后用sparse(rowColIdx(:,1), rowColIdx(:,2), vals, n, n)一次性构造矩阵。这个细节对性能影响非常大。4.2 Levenberg-Marquardt迭代与阻尼因子我在代码包里选择的非线性求解器是Levenberg-Marquardt而不是更简单的Gauss-Newton。原因是因子图里残差函数非线性程度较高尤其是带旋转的位姿追踪问题Gauss-Newton在远离最优值时经常出现增量过大导致发散。LM算法的基本框架是delta -(H lambda * diag(H)) \ g if new_error old_error: 接受delta减小lambda else: 拒绝delta增大lambda阻尼因子lambda的初始值通常设为1e-3每次失败乘以10每次成功除以3到10。具体倍数可以根据问题规模调整这套代码包里我给的是成功除以5失败乘以8实测效果比较平滑。更新量delta需要叠加到当前变量上。这里有一个MATLAB实现的细节向量化更新位姿时旋转部分不能简单做数值加法否则四元数会失去单位模长。代码包里我单独写了updateVariable函数检测type字段如果是pose类型就走特殊更新分支对四元数做归一化或先转旋转矩阵再右乘增量旋转矩阵。4.3 稀疏Cholesky与MATLAB反斜杠的选择增量线性方程的求解MATLAB里最简单的写法是delta -H \ g。MATLAB的\运算符对不同矩阵会自动选择不同算法对稀疏对称正定矩阵会选择Cholesky分解。所以原则上直接用反斜杠就够了。但有几个实际问题一是H可能是半正定的尤其是缺少先验因子或全局参考坐标约束时H会出现零特征值Cholesky分解会失败。这时需要在H上加一个小的正则化项或者固定某些变量节点。二是当变量数量达到数千、因子数量达到数万时直接做完整Cholesky分解的计算量比较大如果只想做增量优化可以使用更新的Cholesky分解但那个在MATLAB里实现比较复杂代码包里暂时没做而是提供了一层接口允许替换成自己写的增量求解器。这里我要强调一点不要过度追求更快在MATLAB原型验证阶段把算法逻辑搞对、把数据流理清优先级远高于性能优化。用反斜杠实现完整Cholesky分解已经可以应对数千变量的问题足够绝大多数算法研究使用。5. 完整代码包的结构设计与调用流程5.1 文件目录怎么组织才不给自己挖坑上一节讨论了代码的技术细节这一节说一下整体的项目结构设计。一个完整代码包目录规划是最容易被忽视但后患无穷的部分。我最终采用的目录结构是这样的factorGraphCode/ ├── core/ % 核心数据结构和求解器 │ ├── nodeStruct.m │ ├── factorStruct.m │ ├── buildGraph.m │ ├── solveGraph.m │ └── updateVariable.m ├── factors/ % 各因子类型的实现 │ ├── priorFactor.m │ ├── odomFactor.m │ ├── gpsFactor.m │ └── preintegrationFactor.m ├── preprocess/ % EKF预处理层 │ ├── ekfPredict.m │ ├── ekfUpdate.m │ └── imuPropagation.m ├── utils/ % 数学工具 │ ├── normalizeAngle.m │ ├── quaternionMul.m │ ├── numericJacobian.m │ └── screwAndStuff.m ├── examples/ % 示例脚本 │ ├── demo2D_odom_gps.m │ ├── demo3D_imu_gps.m │ └── demoVisualSLAM.m ├── test/ % 单元测试和验证脚本 │ ├── testPriorFactor.m │ ├── testOdomFactor.m │ └── testBenchmark.m └── README.md为什么这么分其实很简单core层不依赖具体传感器保证通用性factors层按因子类型扩展加一个新传感器只需要在factors目录下增加一个文件preprocess层是EKF模块和因子图的主体解耦可以单独测试也可以单独复用examples和test分开是因为示例代码面向用户测试代码面向开发者。5.2 一次标准调用流程的伪代码级拆解下面用文字伪代码描述一次完整的因子图构建和优化流程。第一步构建变量节点列表。比如我们有N个关键帧位姿就创建N个变量节点初始化值由EKF输出提供具体为variables struct(id, {}, type, {}, value, {}, cov, {}, fixed, {}); for i 1:N variables(i).id i; variables(i).type pose; variables(i).value ekfPoses(:, i); variables(i).cov ekfCovs{i}; end第二步构建因子节点列表。遍历传感器观测数据对每个观测创建对应的因子节点。以GPS观测为例factors struct(id, {}, type, {}, varIds, {}, measurement, {}, infoMatrix, {}, evalResidual, {}, evalJacobian, {}); for k 1:numGps factors(k).id k; factors(k).type gps; factors(k).varIds gpsVarId(k); factors(k).measurement gpsPosition(:, k); factors(k).infoMatrix inv(gpsCov{k}); factors(k).evalResidual (vars) gpsResidual(vars, factors(k)); factors(k).evalJacobian (vars) gpsJacobian(vars, factors(k)); end第三步调用solveGraph进行优化。solveGraph内部实现LM迭代每次迭代先计算所有因子残差和雅可比再累加稀疏信息矩阵求解增量更新变量。result solveGraph(variables, factors, options);整个流程非常简洁。关键点在于构建因子节点时残差函数和雅可比函数是通过匿名函数捕获因子自身数据的这样在求解器内部不需要区分因子类型统一调用factor.evalResidual(vars)即可。这个设计让代码的可扩展性极强。5.3 数据输入的灵活性从仿真到实跑怎么切换这套代码包还设计了数据输入层的适配我把它单独放到了utils/loadData里。仿真阶段数据可以直接从MATLAB脚本生成实跑阶段数据可能来自ROS bag、CSV日志或自定义格式。为了让代码包不绑定具体数据格式我定义一个中间结构inputData包含time、sensorType、measurement、covariance等字段所有数据都先转换成这个中间结构再交给因子图构建模块。这个做法测试时很方便切换数据源不用动核心代码。对于SLAM定位算法研究这个层次抽象能节省大量时间。6. 实测一个IMUGPS定位场景的因子图与EKF对比6.1 仿真场景设置和代码参数为了验证代码包的实际效果我搭了一个仿真场景。假设载具在一段90秒的轨迹上运动先直线加速再转两个弯最后匀速直线。IMU采样率100HzGPS采样率1Hz。IMU的陀螺噪声标准差0.01 rad/s加速度计噪声标准差0.05 m/s²。GPS位置噪声标准差设为2米。EKF预处理层使用标准15维状态向量位置3、速度3、姿态四元数4、陀螺偏置3、加速度计偏置3状态转移使用IMU推进。因子图优化层的变量节点取GPS时刻对应的关键帧位姿大约90个节点。因子包括先验因子、相邻关键帧之间的里程计等效因子由EKF输出相对增量得到、GPS因子。我用代码包里的demo3D_imu_gps.m脚本跑了一遍优化前先用EKF的位姿估计作为初值然后做了10次LM迭代。6.2 误差对比结果因子图比EKF好不是玄学跑完后的误差结果很典型。EKF最终位置误差大约在3.8米左右因子图优化后的位置误差大约在1.6米左右提升了将近60%。姿态误差也从0.9度降到0.35度。这个提升并不是因为因子图的数学原理神奇而是因为因子图对整个轨迹的GPS观测做了全局优化每个GPS位置约束都能影响到前后相邻状态而EKF每步只利用当前时刻的GPS观测修正当前状态历史GPS信息无法对后期状态产生约束。另一个观察是因子图优化到第3到第4次LM迭代时误差已经下降到接近收敛水平后面迭代只是微调。这说明如果将来要做实时系统在每次新增因子后只做少量迭代或增量优化也能得到足够好的结果。6.3 运行时间与性能瓶颈分析90个节点、约100个因子10次LM迭代整个优化过程在MATLAB R2022b上跑完大约用了0.3秒。时间主要花在雅可比计算和信息矩阵累加上。代码没有做非常严格的向量化因为因子数量较少表现足够好。如果把变量节点扩大到500个因子节点数到1000优化时间会增长到2秒左右其中大部分时间花在稀疏Cholesky分解上。如果继续扩大就需要考虑增量优化和更精细的稀疏排序优化比如AMD重排序。代码包里我预留了一个reorderMatrix选项可以对稀疏矩阵做重排序来提升分解效率。7. MATLAB实现因子图最容易踩的坑7.1 索引对齐问题频繁出现的变量编号错位我在写这套代码包时踩过最深的坑就是MATLAB数组索引和因子图变量id之间的错位。MATLAB数组默认从1开始索引这是常识但因子图变量可能来自多个数据源id不一定连续递增。比如GPS观测可能只关联部分关键帧这些关键帧在变量数组中的位置和GPS因子中的varIds字段可能对不上。一旦出现这种错位优化结果完全错乱而且错误日志很难排查。我的解法是构造一个idToIndex的映射用MATLAB的containers.Map或一个朴素的查找函数确保所有varIds在进入优化器之前都统一转换成数组索引。在因子构建阶段就完成这种转换而不是在求解器中临时转换这样能把错误爆发点集中在构建阶段调试起来容易得多。7.2 信息矩阵累加时的符号与Hessian对称性前面提到雅可比符号方向容易出错这里再补充一个相关但更隐蔽的坑信息矩阵累加时的Hessian对称性检查。理论上每个因子的J^T * info * J都是对称半正定的多个因子累加后仍对称。但由于数值误差或雅可比计算错误H矩阵可能出现轻微不对称。MATLAB里反斜杠对非对称稀疏矩阵会走LU分解而不是Cholesky速度变慢且可能掩盖问题。我在solveGraph里加入了一行代码每次累加后用max(abs(H - H))检查对称性如果超过阈值就跳到调试模式。这样能快速定位是哪个因子导致信息矩阵不对称。7.3 数值微分求雅可比的精度陷阱代码包里提供了numericJacobian.m用中心差分法计算雅可比方便验证解析雅可比。但数值微分的步长选择非常关键步长太大会有截断误差太小会引入浮点舍入误差尤其是残差函数中包含角度归一化等非线性操作时数值微分很容易返回错误的雅可比。建议在需要数值验证时先对残差函数做变量缩放把角度和距离量纲统一到相近尺度再用sqrt(eps)倍的特征尺度作为步长。测试时如果数值雅可比和解析雅可比误差在1e-4以上不要急着说解析雅可比错了先查查角度归一化和旋转表示是否一致。7.4 MATLAB的匿名函数和函数句柄性能最后提一个性能上的坑。在因子图构建时每个因子节点都保存了函数句柄这在MATLAB里虽然灵活但在大量因子时会带来可观的调用开销。实测下来500个因子节点每个因子在每次迭代中调用两次函数句柄总调用次数是一万次MATLAB函数句柄的调用开销比直接内联函数高很多。如果性能要求高建议把因子类型作为数字标识在求解器里用switch分支统一调度残差和雅可比计算而不是用匿名函数。代码包里我保留了函数句柄方案作为默认因为可读性好但在注释里加了性能提示建议在高密度因子场景下切换为switch分支模式。这样做了以后优化时间从2秒降到1.2秒效果还是比较明显的。我个人的体会是做因子图优化MATLAB实际上是个非常好的验证平台关键是别盲目模仿C代码包的架构而是充分利用矩阵化、稀疏矩阵和结构体数组的特性。这套代码包从第一次运行到重构稳定断断续续花了两三周最耗时间的不是求解器而是数据结构和雅可比验证。希望大家在用这套代码包时也能被这些细节逼着把因子图的原理真正吃透。本文还有配套的精品资源点击获取