从概率角度分析VINS后端的优化和滑动窗

前言

这篇文章的主要是内容是为了记录自己对于VINS后端的优化和滑动窗的学习和感悟，主要是作为记录来使用，而不是讲解。公式和图主要是以手绘来完成，来方便自己的记录而不是为了展示。
文章的由来，我之前在深蓝学习SLAM VINS 的时候，遇到了问题。
VINS 的后端是基于滑动窗的优化来进行的，在涉及到余项的处理上时。
仅仅通过余项的二次形式，就可以对于问题进行求解，为什么会引入一个协方差最终呈现出信息矩阵的形式，来对问题进行求解。
最终我对这个问题，进行了思考得出自己的感悟，问题如下：

1. VINS 的后端是基于滑动窗的优化来进行的，在涉及到余项的处理上时。
   仅仅通过余项的二次形式，就可以对于问题进行求解，为什么会引入一个协方差最终呈现出信息矩阵的形式，来对问题进行求解。
2. 如深蓝SLAM中，如果加入协方差只是为了从优化角度上讲，是一种为了测量单位统一的方法的话。那么对于之后，在滑窗时，进行variable elimination操作时，再用概率里面的margin来解释又会在逻辑上感觉更加强行。

综上，经过文献的阅读后，我在此总结自己的收获和理解。并用这篇文章来解释之前阐述的问题。从概率的角度来对这个优化的过程来进行解释。

基础知识内容

这里主要包括的是概率上的一些，为了推导去下文所必须的内容。 内容源自 《notes on Least Square and SLAM》,这里只是用我自己的语言对这个逻辑重新阐述一遍。

我们按照图中的序号一步一步开始，
第一步
首先，我们应该明白的是，咱们对于状态量的估计在概率统计里面应该是一个后验。i.e.P(X|Z)
通过贝叶斯公式就可以完成第一步这个拆解的过程。
第二步
我们这里手写的图来概括。

这里需要补充的内容是，x= x*+ 三角x，x是从优化得到的。
第三步 进行的内容主要内容主要是求解最优化的x

第四步也是最重要的一步 那就是从MLP 变回MAP
这是一个从likehood+prior–> 后验的过程。如果没有这一步，虽然在结果上x*是正确的（这是由于我们对于prior一无所知，所以prior是常数，MLP 和MAP 是一个正比的关系，自然NLL求解出的结果也是一致的），但是从概率的角度上来看，这个推断的过程是不完整的，那么之后的margin在概率上也就解释不通了。所以这一步是必须进行的一步。


至此，基础知识内容结束。

IMU 的推断

因为这是给自己的记录，所以需要声明，这里有一些符号的滥用 请先看左下角说明的符号的意义。
并且因为这是针对，Forster 15 内容进行的imu 预积分，符号的意思请参考 Forster 15 和它的补充文档。
这里对于从VINS 中，对于imu 的推导和 Forster 的方法有所不同，但实际上都可以直接套用文中推导的公式。但是请注意预积分的拟合形式，是欧拉法、中值还是龙格可塔。
还有一个需要注意的就是这里的是信息矩阵不但是J 需要更新，里面包含的协方差矩阵也是需要进行更新的。在Forster内，有提供迭代更新的手法。ESKF也有另一种推导更新的方法。
至于随机游走的估计，也不过是上面的公式的简单扩展。

而之后margin操作就只是建立在后验上的概率处理，并没有逻辑上的不通顺了。因此不在赘述。

至此，我可以说，在逻辑上，这个VINS后端优化是已经通畅了。

reference：
Notes on Least-Square and SLAM
Manipulating on Gaussian Distribution
ESKF
Forster 15 和 它的补充材料