4、因子图建模：变量节点与因子节点的定义、观测模型因子、运动模型因子、先验因子、边缘化与稀疏化
好，咱们进入因子图建模的核心环节。说实话，很多做组合导航的朋友一开始接触因子图，都会被那一堆节点和边搞晕。我当年也是，看了好几篇论文才真正搞明白——其实没那么玄乎，说白了就是一张“谁跟谁有关系”的图。
4.1 变量节点与因子节点：图里的两种“角色”
因子图里只有两种节点。一种是变量节点，另一种是因子节点。变量节点代表我们想估计的状态，比如位置、速度、姿态、IMU零偏等等。因子节点则代表约束条件，比如“这个时刻的GPS观测告诉我位置在这里”、“这两个时刻之间的IMU预积分告诉我相对运动是这样”。
我个人习惯把变量节点画成圆圈，因子节点画成方块。圆圈是“未知数”，方块是“已知条件”。每条边连接一个因子节点和一个变量节点，表示这个因子约束了那个变量。
核心思想：因子图建模的本质，就是把一个复杂的估计问题，拆解成若干个局部约束的乘积。每个因子只关心它连接的那几个变量，互不干扰。
举个例子，一个简单的组合导航因子图可能长这样：
// 伪代码：因子图结构示意
// 圆圈：变量节点 (x0, x1, x2, ...)
// 方块：因子节点
// 先验因子 —— 约束 x0
[PriorFactor] --- (x0)
// 运动模型因子 —— 约束 x0 -> x1
[ImuFactor] --- (x0) --- (x1)
// 观测模型因子 —— 约束 x1
[GpsFactor] --- (x1)
// 运动模型因子 —— 约束 x1 -> x2
[ImuFactor] --- (x1) --- (x2)
// 观测模型因子 —— 约束 x2
[GpsFactor] --- (x2)你看，每个因子只连接它相关的变量。IMU因子连接相邻两个时刻的状态，GPS因子只连接当前时刻的状态。这种局部性，是因子图能够高效求解的关键。
4.2 观测模型因子：把传感器数据“塞”进图里
观测模型因子，说白了就是“传感器告诉我什么，我就信什么，但允许有一定误差”。每个观测因子对应一个残差项：
// 观测因子残差示例（GPS位置观测）
// z_k: GPS观测值
// h(x_k): 预测值（从状态x_k计算出的位置）
// 残差 = z_k - h(x_k)
E_gps = || z_k - h(x_k) ||^2_Σ这里的 Σ 是观测噪声协方差矩阵。Σ 越大，这个因子的“话语权”越小。我在项目中遇到过一个问题：GPS信号质量差的时候，如果还用固定协方差，结果会飘得厉害。后来我改成根据卫星颗数和PDOP值动态调整 Σ，效果好了很多。
实战技巧：对于不同传感器，观测模型因子的设计思路是一样的，只是 h(x) 和 Σ 不同。比如：
- GPS：h(x) 取位置，Σ 根据定位精度设定
- 气压计：h(x) 取高度，Σ 通常较大（气压计容易受天气影响）
- 磁力计：h(x) 取航向，Σ 在室内可能非常大（磁干扰严重）
4.3 运动模型因子：IMU预积分的“桥梁”
运动模型因子，在组合导航里通常就是IMU预积分因子。它连接两个相邻时刻的变量节点，描述它们之间的相对运动关系。
IMU预积分的核心思想是：把两个时刻之间的所有IMU测量值“压缩”成一个相对约束。这样我们就不需要把每个IMU测量时刻都当成一个变量节点，大大降低了图的大小。
// IMU预积分因子残差示意
// ΔR, Δv, Δp: 预积分得到的相对旋转、速度、位置
// R_i, v_i, p_i: 时刻i的状态
// R_j, v_j, p_j: 时刻j的状态
E_imu = || ΔR - (R_i^T * R_j) ||^2
+ || Δv - (R_i^T * (v_j - v_i - g*Δt)) ||^2
+ || Δp - (R_i^T * (p_j - p_i - v_i*Δt - 0.5*g*Δt^2)) ||^2嗯，这里要注意：IMU预积分因子对零偏非常敏感。零偏估计不准，预积分结果就会漂。我曾经踩过这个坑——IMU零偏没初始化好，结果整个因子图优化出来的轨迹都是弯的。后来我学乖了，每次启动时先静止几秒钟，把零偏初值估出来。
4.4 先验因子：给优化一个“起点”
先验因子，就是我们对某个变量节点的“初始猜测”。没有先验因子，优化问题就是欠定的——你想想看，如果没有一个固定的参考点，位置可以随便平移，优化器根本不知道往哪走。
先验因子通常加在第一个时刻的变量节点上：
// 先验因子残差
// x_0: 初始状态
// x_prior: 先验值（比如从GPS单点定位得到的位置）
E_prior = || x_0 - x_prior ||^2_Σ_priorΣ_prior 表示我们对先验的信任程度。如果初始定位很准，Σ_prior 设小一点；如果只是粗略估计，设大一点。我一般会把先验协方差设得比实际观测协方差大一些，让优化器有更多自由度去调整。
避坑指南：千万不要把先验因子设得太“硬”。我曾经在一个项目里，把先验协方差设得特别小，结果后面所有的观测都拉不动这个初始值，整个轨迹都被“钉死”在起点附近。先验的作用是提供参考，不是强制约束。
4.5 边缘化与稀疏化：让图“瘦身”
随着时间推移，变量节点会越来越多。如果不加控制，图会变得无比庞大，求解速度越来越慢。这时候就需要边缘化和稀疏化。
边缘化，就是把不再需要的变量节点从图中移除，但保留它们对剩余变量的影响。具体做法是：对要移除的变量，用Schur补操作，把它的信息“压缩”到与之相连的因子上。
举个例子，假设我们要边缘化时刻0的变量 x0：
// 边缘化前的因子
[Prior] --- (x0) --- [Imu] --- (x1)
// 边缘化x0后，生成一个"虚拟先验"约束x1
[VirtualPrior] --- (x1)这个虚拟先验，就是x0被边缘化后留下的“痕迹”。它包含了x0对x1的所有约束信息。
稀疏化，则是另一种思路。当图变得太稠密时（比如每个变量都跟很多因子相连），求解效率会下降。稀疏化的做法是：把一些弱约束的因子直接丢弃，或者合并成更简单的形式。
我的经验：边缘化和稀疏化是一把双刃剑。边缘化保留了信息但增加了因子之间的耦合度；稀疏化提高了速度但可能损失精度。在实际工程中，我通常的做法是：
- 对于滑动窗口内的变量（比如最近10秒），保留完整因子图
- 对于窗口外的变量，进行边缘化
- 对于长期稳定的传感器（比如GPS），可以适当稀疏化历史观测
说白了，边缘化和稀疏化就是在精度和效率之间找平衡。没有银弹，每个项目都要根据传感器特性、计算资源、精度要求来调参。
这张图展示了一个典型的组合导航因子图结构。x₀ 到 x₃ 是不同时刻的状态变量节点。先验因子约束 x₀，IMU因子连接相邻时刻，GPS因子约束每个时刻的位置。随着时间推移，我们可以边缘化 x₀，生成一个虚拟先验约束 x₁，从而保持图的大小可控。
好了，因子图建模的核心概念就这些。变量节点和因子节点是骨架，观测模型、运动模型、先验因子是血肉，边缘化和稀疏化是让这个系统能长期运行的“新陈代谢”。理解这些，你就掌握了因子图在组合导航中应用的基础。