0 引入摘要：但我们都知道”蝴蝶效应“的客观存在，生活中也会不时出现“一步错，步步错”的情况，这表明很久之前的决定确实会影响当前事物的发展。不难得出结论，由于忽略了历史信息对当前状态的影响，在一阶马尔可夫的假设下进行预测是存在一定偏差的。
卡尔曼滤波立足于一阶马尔可夫假设，认定当前状态只与上一状态有关，使用上一时刻状态 和状态方程 对下一时刻状态进行预测：
但我们都知道”蝴蝶效应“的客观存在，生活中也会不时出现“一步错，步步错”的情况，这表明很久之前的决定确实会影响当前事物的发展。不难得出结论，由于忽略了历史信息对当前状态的影响，在一阶马尔可夫的假设下进行预测是存在一定偏差的。
在卡尔曼滤波中，预测和校正可能并不同步，若长时间缺失观测信息来校正滤波器，会导致误差的不断累积，最终滤波发散。
那么，我们能否从更久远的信息出发，估计当前时刻的状态，并尽可能减少预测误差呢？
可以的，可以的，那就是因子图优化。不负责任的说， 在任何卡尔曼滤波能够应用的问题上，因子图优化都能替代并发挥更好的作用。
1 因子图简介多变量全局函数通常可以因子化为多个局部函数的乘积，其中每个局部函数仅涉及部分变量，从而实现对整体函数的分解。
假设存在一个5变量的全局函数 ，并可进行如下形式的因式分解：
式中， 为全局函数，
如果我们用●来表示函数 ，用◯表示变量 ，再连接相关的 和 ，那么式(1)可以用下图来表示：
上图就是一个典型的因子图。我们直接给出 因子图的定义：
因子图是由 因子节点 （图中●） 、 变量节点 （图中◯）以及 边线 组成，其中边线 为因子节点 和变量节点 之间的连线，且边线总在因子节点和变量节点之间，因此完整的因子图可用 表示。
因子图(Factor Graph, FG)可将系统全局函数抽象为 因子节点(Factor Node) 和 变量节点(State Node) 连接组合而成的图模型，是一种比贝叶斯网络等常规图模型更为精细的模型表示方法。借助 因子节点代表的局部函数 和 变量节点代表的状态变量 ，因子图可清晰描述系统内各子系统的复杂联系，因此，借助因子图可对各类复杂系统特征建模。
2 因子图模型 2.1 基于状态空间模型的因子图建模和卡尔曼滤波一样，我们仍从状态空间模型出发构建因子图模型。
离散时间的非线性状态空间模型一般表示为
注：对非线性状态空间模型概念模糊的可先阅读往期的卡尔曼滤波文章。
非线性模型/扩展卡尔曼滤波-前情提要
滤波与优化，？扩展卡尔曼滤波EKF篇(含原理、案例与代码实现)
2.2 变量节点
每一个需要估计的 未知参数 (状态空间模型的 )，在因子图中就是一个变量节点。
2.3 因子节点每一个对变量节点施加 约束 的 局部信息源 ，在因子图中就是一个因子节点。
它来源于我们的状态空间模型：
观测因子 表达了这样的约束：根据观测模型 ，在状态 下，应观测到 。它将实际的传感器测量作为约束 ，绑定了到对应的状态 上。
运动因子 表达了这样的约束：根据运动模型 和控制输入 ，状态 应该由状态 这样 推导出来。它编码了状态间的 时序依赖 和 过程不确定性 。
先验因子 (Prior Factor) : 一般使用符号 表示先验因子， 连接到初始状态节点 ，代表我们对 初始状态 的信赖度(与滤波的初始状态误差协方差 相关)，它来源于初始状态信息。
过程/运动因子 (Process/Dynamics Factor) : 本文中用符号 表示过程因子节点， 时刻的过程因子表示为 。 连接相邻变量节点 ，对应 状态方程 。
观测因子 (Observation Factor): 用符号 表示观测因子节点，由于同一时刻可能存在多个观测因子，再使用索引 标号每条观测信息 ， 时刻的观测因子表示为 。 连接变量节点 ，这直接对应 观测方程 。
2.4 因子节点的数学表达
前文分析可知，过程/运动因子连接相邻两个时刻的运动状态，形成了相邻变量节点的相对运动约束；观测因子反映当前时刻特定的变量节点信息。从这一角度出发，可对因子节点进行不同的数学表达。
2.4.1 二元因子(Binary factor)二元因子与两个相邻的变量节点有关，用于描述变量节点之间的变化。因此可将运动/过程因子建模为 二元因子节点 。
状态方程描述了状态的时间更新，那么相邻两状态变量节点 和 之间关系可以表示为
根据上式将二元因子节点定义为
式中， 表示代价函数，其具体形式将在 文章2.4部分 给出。
2.4.2 一元因子(Unary factor)一元因子只与相连接的变量节点有关。观测因子一般只连接一个变量节点，因此观测因子可建模为 一元因子节点 。
根据观测方程，观测值 和 之间关系可以表示为
根据上式将一元因子节点定义为
先验因子也可视作对初始状态的观测，也应建模为 一元因子节点 ，定义为
为初始 先验 （初始猜测）。
为因子图待优化的初始变量节点（0时刻的未知状态）。
2.4.3 多元因子(Multiplex factor)此外因子图中还存在多元因子节点，其与多个变量节点相关，模型及定义类似于上述一元因子节点和二元因子节点。本文以入门为主，不再对多元因子做过多赘述。
2.4.4 代价函数的概率表达从前面的分析我们知道，因子节点反映了初始状态、过程或观测的不确定性，那么不难推断， 因子节点实际上是关于概率的函数 。
一般情况下， 我们使用多元高斯分布的概率密度函数来描述这个概率 ：
式中，形如 的公式表示 平方马氏距离 (Mahalanobis Distance)，有 。
为方便进一步处理，将上述因子节点的概率表达中的平方马氏距离转化为2-范数形式，同时略去固定的系数，得到
3 因子图优化原理 3.1 因子图的全局函数在因子图中，任一变量节点的概率密度函数等于与该变量相连的所有因子节点的乘积。
设 为 时刻变量节点，过程模型为 ，且时刻 有 条测量 。则以 为中心的局部图模型的概率密度函数可以写为：
该式表明，变量节点 的信息由其过程先验与所有时刻 的观测因子共同决定。
仿照局部图模型的写法，可得到从初始状态 到 的全局图模型的概率密度函数：
3.2 基于最大后验的参数估计因子图模型的全局函数描述了隐藏状态集和输入、观测之间的联合概率分布关系，理论上当概率为最大时，状态集 也达到最优。
对式(7)计算最大后验估计(Maximum posterior estimation, MAP)，并进一步取负对数，可等价得到一个最小化问题：
将式(4)~式(6)代入 ，可得到：
等价地写成更常见的二次型形式：
3.3 优化求解的方法与策略根据前文的推导我们得到了 图优化问题的最终表示 ：
从式(11)不难看出， 图优化问题本质上属于非线性最小二乘优化问题 ，因此需要选用一种合适的 非线性优化算法 去对 完成具体计算。
3.3.1 优化算法选择非线性优化的求解方法主要分为 线性搜索和信赖域法 (Trust Region Method)。
线性搜索的核心思想是先确定搜索方向，然后确定步长进行迭代，主要包括梯度下降法、高斯牛顿法(Gauss-Newton Method)、莱温伯格-马夸特法(Lewenberg-Marquardt Method)；
信赖域法的核心思想则是先确定最大距离，再确定方向，典型算法为狗腿法(Dogleg Method)。
莱温伯格-马夸特法 (简称L-M法)避免了高斯牛顿法中雅各比矩阵非满秩导致的奇异问题，且能够通过阻尼因子自适应地调节迭代步长和方向，同时具有梯度下降法的速度优势和高斯牛顿法的精度优势，在因子图优化领域已得到广泛应用。
3.3.2 优化策略选择 目前，因子图优化求解有 批量、滑动窗口和增量(iSAM) 这3种优化策略，简单来说批量优化（Batch）：直接对整个 联合优化，优点是最优、可恢复完整协方差，缺点是计算/内存随时间线性增长。
滑动窗口（Fixed-lag smoothing）：只保留最近 个状态，其他状态通过边缘化生成一个先验因子（prior）约束窗口内变量，优点是控制复杂度，缺点是边缘化会使因子结构密化（稠密先验）。
增量平滑（iSAM / iSAM2）：利用稀疏因子结构与 Bayes 树，按需重线性化和局部重因子化，实现实时增量更新且保留稀疏性。
需要 最高精度 且能离线处理，选择 批量优化 ；
需要 在线运行且受限资源 ，又想保留近期最优性，选择 滑动窗口 。
需要 实时、高频更新且长期运行 （保持稀疏与高效）选择 增量平滑 。
4 仿真实战文章模拟一个机器人移动场景，分别应用因子图优化方法和扩展卡尔曼滤波方法估计机器人的运动状态，仿真对比2种方法的性能。
优化方法选用L-M算法，优化策略选用滑动窗口。
4.1 案例设计移动机器人在室内或室外运动时，需要知道自身的 位置 和 航向角 。本案例模拟一个机器人，利用其上搭载的 里程计 （测量线速度 v，角速度 ω）和 GPS （测量位置 x,y）数据来估计机器人轨迹。
具体设计请参考以下文章内容：
请参考文中的仿真案例部分
4.2 仿真结果整体的仿真结果如图所示
位置估计误差与航向估计误差对比可视化如下图所示。
水平位置误差对比可视化如下图所示。
对估计结果进行误差统计，发现EKF方法估计的水平位置误差均方根为0.3590；FGO方法估计的水平位置误差均方根为0.2194，相比之下 图优化方法的位置估计误差降低了38.9% ，因此因子图优化方法相比于传统滤波方法具有极大的精度优势。
4.3 部分仿真代码主脚本main.m
%% MATLAB script: Circular trajectory simulation + EKF state estimation + FGO state estimation% Copyright (C) 2025, 马尔科夫曼贝叶高斯, all rights reserved.
%% Set simulation parameters
% Simulation parameters
dt =0.1; % Time step (s)
N =500; % Number of steps
R =20; % Circle radius (m)
omega_true=0.1; % True angular velocity (rad/s)
v_true= R * omega_true; % True linear speed (m/s)
% Noise characteristics
overlay_std =0.05; % Speed sensor noise (m/s)
omega_std =0.005; % Yaw rate noise (rad/s)
gps_std =1.0; % GPS position noise (m)
% Preallocate arrays
true_state =zeros(3, N); % [x; y; theta]
v_meas =zeros(1, N);
omega_meas =zeros(1, N);
gps_meas =zeros(2, N);
% Initial condition: start at angle 0
phi =zeros(1,N);
phi(1) =0;
% 注意：程序基于极坐标计算圆弧轨迹轨迹，航向和极坐标角度相差为pi/2
true_state(:,1) = [R;0;phi(1)+pi/2];%定义状态向量：水平位置X、Y，航向θ
% Generate true trajectory and measurements
rng(0);
fork =2:N
% Update true heading
phi(k) = phi(k-1) + omega_true*dt;%极坐标角度
% Compute true position on circle
true_state(1,k) = R *cos(phi(k));
true_state(2,k) = R *sin(phi(k));
true_state(3,k) = phi(k) +pi/2;%航向
% Odometry measurements: linear speed + yaw rate
v_meas(k) = v_true+ overlay_std *randn;
omega_meas(k) = omega_true+ omega_std *randn;
% GPS measurement: noisy x, y
gps_meas(:,k) =true_state(1:2,k) + gps_std *randn(21);
end
% EKF initialization
x_ekf =zeros(3, N);
P =eye(3);
Q =diag([0.1^20.1^20.01^2]); % process noise covariance
Rk = gps_std^2*eye(2); % measurement noise covariance
x_ekf(:,1) =true_state(:,1); % initial state estimate
% State transition function
motion_model = @(x, u) [ ...
x(1) + u(1)*dt *cos(x(3)); ...
x(2) + u(1)*dt *sin(x(3)); ...
x(3) + u(2)*dt ...
];
%% EKF loop
fork =2:N
% Prediction
u = [v_meas(k); omega_meas(k)];
x_pred = motion_model(x_ekf(:,k-1), u);
% Jacobian F
F = [10, -u(1)*dt*sin(x_ekf(3,k-1));
01, u(1)*dt*cos(x_ekf(3,k-1));
001];
P_pred = F * P * F' + Q;
% Update with GPS
H = [100;010];
z = gps_meas(:,k);
y = z - H * x_pred;
S = H * P_pred * H' + Rk;
K = P_pred * H' / S;
x_ekf(:,k) = x_pred + K * y;
P = (eye(3) - K * H) * P_pred;
end
%% FGO loop
W_len=20;%窗口长度
mu_prior=x_ekf(:,1);
P_prior =diag([10^210^2, (1e-2)^2]);
% 配置选项（开启雅可比估计更快）
% opts = optimoptions('lsqnonlin','Display','off','SpecifyObjectiveGradient',false);
opts = optimoptions(@lsqnonlin,'Algorithm','levenberg-marquardt','MaxIterations',100,'StepTolerance',1E-6,'FunctionTolerance',1E-6);
%滑动窗口
x_fgo =zeros(3, N);
fort =1: N
% 1) 窗口索引
ift
idx_window =1:t;
else
idx_window = t-W_len+1: t;
end
W =numel(idx_window);
% 2) 收集窗口数据
v_win = v_meas(idx_window);
omega_win = omega_meas(idx_window);
gps_win = gps_meas(:,idx_window);
% 3) 构造初值
%X0_win = reshape(x_ekf(:,idx_window), 3*W, 1);
X0_win = initialGuessWindow(mu_prior, v_win, omega_win, gps_win, dt, Q(1:21:2), Rk);
% 4) 构造残差函数，把 mu_prior/P_prior“捕获”进去
fun = @(X) slidingResiduals( ...
X, v_win, omega_win, gps_win, dt, Q, R, mu_prior, P_prior);
% 5) 优化
Xopt = lsqnonlin(fun, X0_win, , , opts);
% 6) 边缘化第一帧，更新先验
iflength(Xopt) ==3*W_len
[P_prior, mu_prior] = marginalizeFirstNode(Xopt, W, fun);
end
mu_prior = mu_prior(1:3); % 新窗口第一帧的 3×1 均值
P_prior = P_prior(1:31:3); % 新窗口第一帧的 3×3 协方差
% 7) 存储本次最优结果
x_fgo(:, idx_window) =reshape(Xopt,3, W);
end
%% Plotting results
figure;holdon; grid on;
plot(true_state(1,:),true_state(2,:), 'k-', 'LineWidth',1.5);
plot(gps_meas(12:end), gps_meas(22:end), 'r.', 'MarkerSize',8);
plot(x_ekf(1,:), x_ekf(2,:), 'b--', 'LineWidth',1.5);
plot(x_fgo(1,:), x_fgo(2,:), '-', 'LineWidth',1.5);
legend('True trajectory', 'GPS measurements', 'EKF estimate', 'FGO estimate');
title('Circular Trajectory: EKF Position Estimation');
axis equal;
% Error analysis: position and heading errors
errors_ekf =true_state - x_ekf; % 3xN error_ekf matrix
errors_fgo =true_state - x_fgo; % 3xN error_ekf matrix
theta_ekf_error = wrapToPi(errors_ekf(3,:)); % Heading error normalized
theta_fgo_error = wrapToPi(errors_fgo(3
figure;
subplot(311);
plot(1:N, errors_ekf(1,:), 'LineWidth',1.2);
holdon
plot(1:N, errors_fgo(1,:), 'LineWidth',1.2);
ylabel('Error in x (m)'); grid on;
legend('EKF','FGO')
subplot(312);
plot(1:N, errors_ekf(2,:), 'LineWidth',1.2);
holdon
plot(1:N, errors_fgo(2,:), 'LineWidth',1.2);
ylabel('Error in y (m)'); grid on;
legend('EKF','FGO')
subplot(313);
plot(1:N, theta_ekf_error, 'LineWidth',1.2);
holdon
plot(1:N, theta_fgo_error(1,:), 'LineWidth',1.2);
ylabel('Error in heta (rad)'); xlabel('Time step'); grid on;
legend('EKF','FGO')
% Plot Euclidean position error and heading error magnitude figure;
pos_ekf_error =sqrt(errors_ekf(1,:).^2+ errors_ekf(2,:).^2); % Euclidean position error
pos_fgo_error =sqrt(errors_fgo(1,:).^2+ errors_fgo(2,:).^2
figure
plot(1:N, pos_ekf_error, 'LineWidth',1.2);holdon; grid on;
holdon
plot(1:N, pos_fgo_error, 'LineWidth',1.2);
ylabel('Position error (m)'); xlabel('Time step');
legend('EKF','FGO')
知乎： 因子图优化 - 知乎
论文：Covariance Estimation for Factor Graph Based Bayesian Estimation
书籍：高翔，视觉SLAM十四讲
来源：寂寞的咖啡