从零手写VIO——（四）基于滑动窗口算法的 VIO 系统：可观性和一致性（上）舒尔补_人工智能

从零手写VIO——（四）基于滑动窗口算法的 VIO 系统：可观性和一致性（上）舒尔补

从高斯分布到信息矩阵

某个状态
$\xi$ ，以及一次与该状态相关的观测
$r_i$ 。由于噪声的存在，观测服从
$p(r_i|\xi)$ 的概率分布，可以直观理解为：在状态
$\xi$ 下，呈现观测
$r_i$ 的概率，当此概率越大说明该观测越准确。多次观测时，假设各个观测之间相互独立，则多个测量
$r=(r_1,r_2,...,r_n)^T$ 构成的似然概率为：

从零手写VIO——（四）基于滑动窗口算法的 VIO 系统：可观性和一致性（上）舒尔补 — (1)

如果知道机器人状态的先验信息
$p(\xi)$ ，如 GPS，车轮码盘信息等，则根据贝叶斯法则，可以求得后验概率：

通过最大后验概率估计，获得系统状态的最优估计：

在之前我写的卡尔曼滤波中公式部分同样使用了最大后验概率估计。在本问题中，在系统状态 $\xi$ 有噪声的观测 $r_i$ 表示成概率分布的形式；在已知状态的先验信息，可能是其他传感器得到的信息，现在想融合相机信息，使用 Bayes 法则，求出后验概率 $p(\xi|r)$ 。后验公式(2)状态量与分母无关，最大后验变成：

即对等式右侧取对数 $\log$ ，根据对数的性质拆开，求其负值的最小值。这里取对数的原因也很简单，高斯分布中有
$\exp$ （高斯分布高维形式：
$p(x)=\frac{1}{\sqrt{(2\pi)^N\det(\Sigma)}}\exp(-\frac{1}{2}(x-\mu)^T\Sigma^{-1}(x-\mu))$ ），舍去系数、负负为正也就有了(7)式的形式：

(5)

假设观测值服从多元高斯分布：

故有：

此最小二乘的求解可以使用增量方程：(下式应该很好理解，
$\delta \xi$ 是系统状态量的增量，类比于经典方程的
$\Delta x$ ；
$\Sigma^{-1}$ 是信息矩阵，也是协方差的逆)

(8)

多元高斯分布

多元高斯分布（The Multivariate normal distribution）www.cnblogs.com 从零手写VIO——（四）基于滑动窗口算法的 VIO 系统：可观性和一致性（上）舒尔补零均值的多元高斯分布：
$p(x)=\frac{1}{Z}\exp(-\frac{1}{2}x^T\Sigma^{-1}x)$ ，
$\Sigma$ 是协方差矩阵，协方差矩阵的逆记作
$\Lambda =\Sigma^{-1}$ 。三维变量的协方差矩阵为：

(9)

若变量间相互独立，那么除主对角线以外元素都为 0。在上讲中用连续时间下的狄拉克函数
$\delta(t_1-t_2)$ 描述相互独立的关系。(9)式中用
$\Sigma_{ij}=E(x_ix_j)$ 为对应元素求期望。

Examples^[1]

Example 1

根据此描述，写出协方差矩阵：

其中：

此外
$\Sigma_{22}=\sigma_2^2$ ，
$\Sigma_{33}=w_3^2\sigma_2^2+\sigma_3^2$ ，下面计算非对角元素：

公式可以通过方差的性质直接写出：
$\Sigma_{y_1}=w_1^2\sigma_2^2+\sigma^2_1$ 。上述这部分在论文中是这样书写的：（K就是
$\Sigma$ 协方差）

随后计算该协方差矩阵的逆：

推导应该是很容易理解的，
$y_1\sim N(w_1y_2,\sigma_1),y_2\sim N(0,\sigma_2),y_3\sim N(w_3y_2,\sigma_3)$ ，且
$y_2$ 是
$y_1,y_3$ 的条件。然后就有个上面的式子，再经通分后写成矩阵形式就有了
$\frac{1}{Z'}\exp(-\frac{1}{2}Y^TK^{-1} Y)$ 的形式，式中的
$K^{-1}$ 就是协方差矩阵的逆，也称作信息矩阵。可以注意到，它的
$(1,3)(3,1)$ 位置是0，表示
$y_1,y_3$ 关于
$y_2$ 条件独立。若室内外温度正相关
$\propto$ （
$w_i>0″> ）： </p><ul><li>协方差中非对角元素 <img decoding=$ 表示在 $y_3$ 发生的前提下，元素 $y1,y2$ 正相关。

Example 2

图中可见是两个变量控制一个变量，比如三角化，用两个相机 pose 计算特征三维坐标的深度：

论文中是这样↓的：

但是论文对逆矩阵
$K^{-1}$ 的推导中有一个小的错误就是提取出
$-\frac{1}{2}$ 后，位于矩阵
$K^{-1}_{11},K^{-1}_{33}$ 两个位置的分母并没有将 2 提出来，其他的没有问题，可以自己简单推一下。

虽然 $y_1,y_2$ 不相关，但是他们的信息矩阵对应元素 $\Lambda_{13}$ 并不为 0。
而当 $\Lambda_{ij}>0″> 时，即对应信息矩阵变量 <img decoding=$ 在另一变量发生的前提下，成负相关。本例中从公式 $y_2=w_1y_1+w_3y_3+v_2$ 就可以看出，当 $y_2$ 确定时， $y_1$ 越大， $y_3$ 越小。

从 Example 1 去除变量 $y_3$

由于公式是这样的：

$y_1$ 和
$y_2$ 的取值都与
$y_3$ 无关，故可以直接在协方差矩阵上把有关于
$y_3$ 的全部删去：
从零手写VIO——（四）基于滑动窗口算法的 VIO 系统：可观性和一致性（上）舒尔补

(16)

矩阵对称的不要忘记！这样就变成了一个
$2×2$ 的矩阵。对于信息矩阵：

则是删去与
$y_3$ 相关的所有项，在矩阵中就是紫蓝色表示的。可是实际情况下并不会把变量的项用颜色在矩阵中分类，所以引入 Sochur 和边缘化。

舒尔补应用：边际概率，条件概率

舒尔补^[2]定义

给定任意的分块矩阵
$M$ ，如下所示：

如果，矩阵块 $D$ 是可逆的，则 $A-BD^{-1}C$ 称之为 $D$ 关于 $M$ 的舒尔补。
如果，矩阵块 $A$ 是可逆的，则 $D-CA^{-1}B$ 称之为 $A$ 关于 $M$ 的舒尔补。

Schur complementen.wikipedia.org

是不是很熟悉，在十四讲第二版的第 248-251 页，求解稀疏矩阵时就用到了舒尔补进行边缘化，将观测点 marginalize 使得改进了原先使用的 EKF 方法，使得 BA 能够实时计算。

如何得到舒尔补的形式

将
$M$ 矩阵变成上三角或者下三角形过程中，就会得到舒尔补：

其中：
$\Delta_A=D-CA^{-1}B$ 。联合起来，将
$M$ 变成对角矩阵：

反过来还能恢复成矩阵
$M$ ：

舒尔补应用于多元高斯分布

设多元变量
$x$ 服从高斯分布，且由两部分组成：
$x=\begin{bmatrix}a&b\end{bmatrix}^T$ ，变量构成的协方差矩阵：

其中
$A=cov(a,a),D=cov(b,b),C=cov(a,b).$ 概率分布为：

(23)

$p(a)$ 是边际概率，
$p(b|a)$ 是条件概率。 从上式可知：
$\it p(a)\sim \cal N\it(0, A),p(b|a)\sim \cal N\it(CA^{-1}a,\Delta_A)$ 。即边际概率的协方差直接取矩阵块就成，条件概率的协方差是
$a$ 对应的舒尔补。从上式(23)可以得到信息矩阵
$\Lambda$ ：

可以总结出 $P(a),P(b|a)$ 的信息矩阵：

条件概率 $P(b|a)$ 的信息矩阵为： $\Delta_A^{-1}=\Lambda_{bb}$ 。
边际概率 $P(a)$ 的信息矩阵为： $A^{-1}=\Lambda_{aa}-\Lambda_{ab}\Lambda_{bb}^{-1}\Lambda_{ba}$ 。 $A^{-1}$ 是边际概率的信息矩阵， $\Lambda_{aa}$ 是联合信息矩阵的部分矩阵块。