从零开始学习VIO笔记 --- 第四讲：滑动窗口（基于滑动窗口算法的 VIO 系统：可观性和一致性）

前一讲中有提到整个残差函数由三部分组成，其中第一部分就是滑动窗口的先验，这一讲主要就是讲如何在去除一帧信息的时候向整个残差添加约束。

一. 从高斯分布到信息矩阵

1.1 高斯分布

首先对SLAM问题进行一个概率建模，利用贝叶斯的理论。
考虑某个状态 $\mathbf{\xi }$ , 以及一次与该变量相关的观测 ${\mathbf{r}}_i$。由于噪声的存在，观测服从概率分布 $p({\mathbf{r}}_i|\mathbf{\xi })$。多次观测时，各个测量值相互独立，则多个测量 $\mathbf{r}=({\mathbf{r}}_1,...,{\mathbf{r}}_n{)}^T$ 构建的似然概率为：
$$p(\mathbf{r}\mid \mathbf{\xi })=\prod _{i}p\left({\mathbf{r}}_i\mid \mathbf{\xi }\right)$$

如果知道机器人状态的先验信息 $p(\mathbf{\xi })$，如 GPS, 车轮码盘信息等，则根据 Bayes 法则，有后验概率：$$p(\mathbf{\xi }\mid \mathbf{r})=\frac{p(\mathbf{r}\mid \mathbf{\xi })p(\mathbf{\xi })}{p(\mathbf{r})}$$
通过最大后验估计，获得系统状态的最优估计：
$${\mathbf{\xi }}_{\mathrm{M}\mathrm{A}\mathrm{P}}=\arg \underset{\mathbf{\xi }}{\max }p(\mathbf{\xi }\mid \mathbf{r})$$

由于后验公式中分母跟状态量无关，舍弃。最大后验变成了：
$${\mathbf{\xi }}_{\mathrm{M}\mathrm{A}\mathrm{P}}=\arg \underset{\xi }{\max }\prod _{i}p\left({\mathbf{r}}_i\mid \mathbf{\xi }\right)p(\mathbf{\xi })$$
即
$${\mathbf{\xi }}_{\mathrm{M}\mathrm{A}\mathrm{P}}=\arg \underset{\mathbf{\xi }}{\min }\left[-\sum _i\log p\left({\mathbf{r}}_i\mid \mathbf{\xi }\right)-\log p(\mathbf{\xi })\right]$$

如果假设观测值服从多元高斯分布:$$p\left({\mathbf{r}}_i\mid \mathbf{\xi }\right)=\mathcal{N}\left({\mathbf{\mu }}_i,{\mathbf{\Sigma }}_i\right),p(\mathbf{\xi })=\mathcal{N}\left({\mathbf{\mu }}_{\xi },{\mathbf{\Sigma }}_{\xi }\right)$$
则有：$${\mathbf{\xi }}_{\mathrm{M}\mathrm{A}\mathrm{P}}=\underset{\xi }{\mathrm{argmin}}\sum _i{\Vert {\mathbf{r}}_i-{\mathbf{\mu }}_i\Vert }_{{\Sigma }_i}^2+{\Vert \mathbf{\xi }-{\mathbf{\mu }}_{\xi }\Vert }_{{\Sigma }_{\xi }}^2$$

这个最小二乘的求解可以使用上节课的解法：
$${\mathbf{J}}^{\top }{\mathbf{\Sigma }}^{-1}\mathbf{J}\delta \mathbf{\xi }=-{\mathbf{J}}^{\top }{\mathbf{\Sigma }}^{-1}\mathbf{r}$$