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

---

从高斯分布到信息矩阵

某个状态

，以及一次与该状态相关的观测

。由于噪声的存在，观测服从

的概率分布，可以直观理解为：在状态

下，呈现观测

的概率，当此概率越大说明该观测越准确。多次观测时，假设各个观测之间相互独立，则多个测量

构成的似然概率为：

(1)

如果知道机器人状态的先验信息

，如 GPS，车轮码盘信息等，则根据贝叶斯法则，可以求得后验概率：

(2)

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

(3)

在之前我写的卡尔曼滤波中公式部分同样使用了最大后验概率估计。在本问题中，在系统状态

有噪声的观测

表示成概率分布的形式；在已知状态的先验信息，可能是其他传感器得到的信息，现在想融合相机信息，使用 Bayes 法则，求出后验概率

。

芝士奶盖：卡尔曼滤波 —— 各路大神和自己的总结​zhuanlan.zhihu.com

后验公式(2)状态量与分母无关，最大后验变成：

(4)

即对等式右侧取对数

，根据对数的性质拆开，求其负值的最小值。这里取对数的原因也很简单，高斯分布中有

（高斯分布高维形式：

）

，舍去系数、负负为正也就有了(7)式的形式：

(5)

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

(6)

故有：

(7)

此最小二乘的求解可以使用增量方程：(下式应该很好理解，

是系统状态量的增量，类比于经典方程的

；

是信息矩阵，也是协方差的逆)

(8)

多元高斯分布

多元高斯分布（The Multivariate normal distribution）​www.cnblogs.com

零均值的多元高斯分布：

，

是协方差矩阵，协方差矩阵的逆记作

。三维变量的协方差矩阵为：

(9)

若变量间相互独立，那么除主对角线以外元素都为 0。在上讲中用连续时间下的狄拉克函数

描述相互独立的关系。(9)式中用

为对应元素求期望。

Examples ^[1]

Example 1

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

(10)

其中：

(11)

此外

，

，下面计算非对角元素：

(12)

公式可以通过方差的性质直接写出：

。

上述这部分在论文中是这样书写的：（K就是

协方差）

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

推导应该是很容易理解的，

，且

是

的条件。然后就有个上面的式子，再经通分后写成矩阵形式就有了

的形式，式中的

就是协方差矩阵的逆，也称作信息矩阵。可以注意到，它的

位置是0，表示

关于

条件独立。

若室内外温度正相关

（

）：

• 协方差中非对角元素
  表示两变量正相关。
• 信息矩阵中非对角元素为负数，为零。
  表示在
  发生的前提下，元素
  正相关。

Example 2

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

(13)

(14)

论文中是这样↓的：

但是论文对逆矩阵

的推导中有一个小的错误就是提取出

后，位于矩阵

两个位置的分母并没有将 2 提出来，其他的没有问题，可以自己简单推一下。

• 虽然
  不相关，但是他们的信息矩阵对应元素
  并不为 0。
• 而当
  时，即对应信息矩阵变量
  在另一变量发生的前提下，成负相关。本例中从公式
  就可以看出，当
  确定时，
  越大，
  越小。

从 Example 1 去除变量

由于公式是这样的：

(15)

和

的取值都与

无关，故可以直接在协方差矩阵上把有关于

的全部删去：

(16)

矩阵对称的不要忘记！这样就变成了一个

的矩阵。对于信息矩阵：

(17)

则是删去与

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

---

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

舒尔补 ^[2] 定义

给定任意的分块矩阵

，如下所示：

(18)

• 如果，矩阵块
  是可逆的，则
  称之为
  关于
  的舒尔补。
• 如果，矩阵块
  是可逆的，则
  称之为
  关于
  的舒尔补。

Schur complement​en.wikipedia.org

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

如何得到舒尔补的形式

将

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

(19)

其中：

。联合起来，将

变成对角矩阵：

(20)

反过来还能恢复成矩阵

：

(21)

(22)

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

设多元变量

服从高斯分布，且由两部分组成：

，变量构成的协方差矩阵：

其中

概率分布为：

(23)

是

边际概率，

是

条件概率。

从上式可知：

。即边际概率的协方差直接取矩阵块就成，条件概率的协方差是

对应的舒尔补。

从上式(23)可以得到信息矩阵

：

(24)

可以总结出

的信息矩阵：

• 条件概率
  的信息矩阵为：
  。
• 边际概率
  的信息矩阵为：
  。
  是边际概率的信息矩阵，
  是联合信息矩阵的部分矩阵块。

用求得的结论验证 example 1 的边缘化形式：

从联合分布

中边缘化掉变量

，即

对应的信息矩阵为：

(25)

与删去颜色得到的完全一致！

总结部分我直接截了贺博课件的图如下：

参考

1. ^David Mackay. "The humble Gaussian distribution". In: (2006) https://people.montefiore.uliege.be/geurts/Cours/AML/Readings/humble.pdf
2. ^Schur Complement https://en.wikipedia.org/wiki/Schur_complement