最近我在研究VIO相关的算法，特在此进行一些简单地总结，由于该部分的理论比较复杂，我可能在理解上有一些错误的地方，烦请大佬们轻拍，欢迎指正，非常感谢。

1. 从条件概率开始

vio的内容已经基本都学完了，到此其中涉及非常多的知识，零零散散，没有真正的串在一起。所以理清vio的来龙去脉对于理解整个系统非常有帮助。在此将花一些时间对VIO系统进行整理。

不妨从基本的条件概率来推导一下VIO

首先，我们待求的状态量为机器人的位姿和路标点，记作：$x=[x_1,\cdots ,x_N],y=[y_1,\cdots ,y_M]$

我们要做的就是通过测量值$z$和已知的控制量$u$来求解这些状态量，当然这个测量对于视觉SLAM而言多数情况下是图像，在VIO系统中控制量$u$对应的是预积分。

根据这些测量得到状态的估计，可以记成条件概率的形式：
$$P(x,y|z,u)\text{\hspace{1em}(1)}$$

不用多说，直接上贝叶斯法则：
$$P(x,y|z,u)=\frac{P(z,u|x,y)P(x,y)}{P(z,u)}\text{\hspace{1em}(2)}$$

多数情况下我们要处理的变量是$x,y$，而$P(z,u)$是一个常量，所以可以忽略掉。因此得到
$$P(x,y|z,u)\propto P(z,u|x,y)P(x,y)\text{\hspace{1em}(3)}$$

式子（3）右边第一项表示似然，其意义是在$x,y$的条件下得到怎样的$z,u$，右边第二项是先验。

我们要做的就是从（2）式中找到在$z,u$条件下的最有可能的$x,y$，从概率的角度来讲，就是求$P(x,y|z,u)$最大的置信度。因此有：
$$(x,y{)}_{MAP}^{\ast }=\arg \max P(x,y|z,u)=\arg \max P(z,u|x,y)P(x,y)\text{\hspace{1em}(4)}$$

从实际模型来说，（4）式中的$P(z,u|x,y)$对应的是测量，多数情况下在SLAM问题中对应的是图像的测量，$P(x,y)$对应的是状态量的预测。

不妨从文字叙述的角度再理解一下（4）式，我们想通过条件概率，获得状态量$x,y$的最优估计，由于很难直接通过$P(x,y|z,u)$进行建模求解，因此将其通过贝叶斯法则进行转换，转换成求其最佳的测量和最佳的预测。而（4）式取得置信度最大时对应的状态量$x,y$，就是我们认为当前条件下的最优状态。

另外如果没有先验，那就是完全从测量数据中估计当前的最优状态。那么问题就变成了：
$$(x,y{)}_{MAP}^{\ast }=\arg \max P(z,u|x,y)\text{\hspace{1em}(5)}$$

2. 最大似然估计的高斯建模

现在不妨考虑（5）式的某一次观测：
$$z_{k,j}=h(y_j,x_k)+v_{k,j}\text{\hspace{1em}(6)}$$

式（6）对于（5）的概率表示是从高斯分布的假设下进行建模的。因此对于当前的单次测量写成条件概率的为：
$$P(z_{j,k}|x_k,y_j)=N(h(y_j,x_k),Q_{k,j})\text{\hspace{1em}(7)}$$

常规操作，对（7）式子取写成概率密度的形式，并取负对数：
$$\begin{gathered} (x_k,y_j{)}^{\ast }=\arg \max N(h(y_j,x_k),Q_{k,j}) \\ =\arg \min ((z_{k,j}-h(x_k,y_j){)}^T{Q}_{k,j}^{-1}(z_{k,j}-h(x_k,y_j){)}^T)\text{\hspace{1em}(8)} \end{gathered}$$

因此，最大化似然估计，就变成了最小化测量误差了，哈哈，总算有点儿清晰了。

很明显，如果我们在只有测量的情况下进行进行状态估计，当我们使用高斯噪声进行建模的时候，我们最后的条件概率的最大似然估计就等于最小化测量误差，更直白的说就是，计算出一个最优的状态量$x_k,y_j$使得似然估计取得最大值。

其中上面的噪声$Q_{k,j}$也是一个相当重要的东西，不妨也花些时间去探究一下。在实际的SLAM问题中，它多数情况是一个协方差矩阵，它衡量了测量量之间的相关性，对于像素测量而言像素测量值$u,v$是无关的，因此$Q_{k,j}$就是一个对角矩阵，对角上的每个值反应了当前测量分量的准确度。这个东西非常的重要，不管是滤波还是优化方法，它都是灵魂。

3. 批量的最大似然估计

实际上对于SLAM系统，通常都是假设各个时刻的输入$u$，各个观测$z$是他们之间都是相互独立的。因此可以对似然进行进一步的因式分解：
$$P(z,u|x,y)=\underset{k}{\Pi }P(u_k|x_{k-1},x_k)\underset{k,j}{\Pi }P(z_{k,j}|x_k,y_j)\text{\hspace{1em}(9)}$$

依然进行常规操作，对概率密度函数取负对数，则最终可以得到如下误差形式：
$$\begin{gathered} e_{u,k}=x_k-f(x_{k-1},u_k) \\ {e}_{z,j,k}=z_{k,j}-h(x_k,y_j)\text{\hspace{1em}(10)} \end{gathered}$$

由于独立性的一些假设，我们得到了一个结论就是，在高斯噪声的假设下，最大化似然估计，相当于是最小化所有的误差项的和。也就是下式：
$$\min J(x,y)=\sum _k{e}_{u,k}^T{R}_k^{-1}{e}_{u,k}+\sum _k\sum _j{e}_{z,k,j}^T{Q}_{k,j}^{-1}{e}_{z,k,j}\text{\hspace{1em}(11)}$$

以上过程 其实也是最小二乘求最优状态量的由来。

4.从VIO系统看批量状态估计

对于式子（9）中的$u$可以从IMU的积分中得到。更具体而言对于在VIO系统中，通过预积分对$u$进行不断地累积计算，最终会得到一个$u$的预测，由于IMU积分出来的结果与图像匹配得到的结果之间会有偏差，因此需要进行优化使得$e_{u,k}$尽量的小。前面我说过噪声在这样的优化系统中起到决定性作用，因此当引入预积分求输入量$u$之后，其方差计算就变得复杂了很多。由于IMU本身是一个非线性的系统，而高斯分布传递只能在线性情况下有效，因此如众多VIO系统的处理方法一样，对IMU的预积分进行一阶泰勒近似，然后在预积分的过程中，通过线性化的方式对噪声进行传递。直到当有一个视觉测量到来的时候，将联合IMU的预积分误差与图像的测量误差一并进行优化。

以上就是VIO系统的理论来源。

说到底vio中的非线性优化，不过就是一个按照协方差分配误差的优化器。

对于非线性优化，当误差项太多时其优化变得非常耗时，因此在VINS-Mono中采用滑窗优化的方法，不再是优化之前的所有状态，而是在一个固定大小的窗口内，优化一个固定数量的状态量，这样一定程度保证了精度，另一方面保证了速度。

5. 卡尔曼滤波的来源

对于式（1）如果遵从马尔科夫链假设，那么可以从贝叶斯的角度进行推导，最终再引入高斯噪声假设，就可以推导出卡尔曼滤波器。
关于这个问题的推导各种资料实在太全了，我就不在此赘述。

6. 总结

从概率的角度简单地探索了一下非线性优化的由来，实际上它只是对条件概率的另一种具体表示，不同于卡尔曼滤波，非线性优化的方式是通过最大化似然估计，联合一批数据进行优化，而卡尔曼滤波器建立在马尔科夫链假设之下。

参考资料

1. 视觉SLAM十四讲
2. 概率机器人