INS+GNSS的图优化关键点
图优化是什么
图优化和KF
KF
1、由于 KF 的推导过程含两个假设，即一阶马尔可夫假设和高斯噪声假设，那么在其他 KF 的扩充版本中，这两个假设始终存在。
对于高斯假设，多传感器融合中各种传感器的噪声类型多样且复杂，并不一定满足高斯噪声假设，因此仍采用该假设会存在一定的问题。
其次对于马尔可夫假设，实际上当前状态的影响因素并不是单单只受到上一时刻的状态影响，而可能是之前的很多时刻，那这样的假设会在一定程度上丢失可观测性以及造成误差的累积。
2、KF 是线性系统。尽管如 EKF、UKF 等都是为了解决非线性问题，但实际上都是采用非线性线性化的形式，会存在线性化误差。除此之外，以 EKF 为主的融合算法实际上仍然是一种次优化算法，会在一定程度上存在发散的问题。
随着多源传感器的引入，任务时间的增长，状态向量也在不断变大，此时 EKF中的协方差矩阵将会以状态向量长度的平方的量级变大，这将造成非常大的内存消耗，且计算上十分耗时。
图优化
图优化仅保存残差大小量级的信息矩阵，因此随着地图的增大、待估参数维数增多，并不会造成内存过度消耗的情况。且在构建图优化时，每种传感器的观测值约束可以看作不同的因子，在每种因子中单独实现对状态的约束，因此也称为因子图优化（Factor Graph Optimization，FGO）。
图优化要解决什么
一个优化问题可以看成通过调整参数将一大堆各种各样的残差降到最小，因此，残差的提供是至关重要的，一个残差的构建离不开残差的数学定义以及关联的参数。
GNSS+INS中的图优化问题
流程：
- 待优化参数x，也叫优化状态向量，优化因子，图中的X；X参数块
- 残差，图中的1（rgnss ）、3（rpre）；残差块 +雅可比矩阵，雅可比矩阵会出现在残差中；
- 残差因子对应的信息矩阵，图中的2和4；
- 边缘化因子的先验信息。图中的5；