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

122
0
2020年10月6日 20时48分

44

从高斯分布到信息矩阵

某个状态 [公式] ,以及一次与该状态相关的观测 [公式] 。由于噪声的存在,观测服从 [公式] 的概率分布,可以直观理解为:在状态 [公式] 下,呈现观测 [公式] 的概率,当此概率越大说明该观测越准确。多次观测时,假设各个观测之间相互独立,则多个测量 [公式] 构成的似然概率为:

 

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

如果知道机器人状态的先验信息 [公式] ,如 GPS,车轮码盘信息等,则根据贝叶斯法则,可以求得后验概率:

 

从零手写VIO——(四)基于滑动窗口算法的 VIO 系统:可观性和一致性(上)舒尔补插图(7)
(2)

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

从零手写VIO——(四)基于滑动窗口算法的 VIO 系统:可观性和一致性(上)舒尔补插图(8)
(3)

在之前我写的卡尔曼滤波中公式部分同样使用了最大后验概率估计。在本问题中,在系统状态 [公式]有噪声的观测 [公式] 表示成概率分布的形式;在已知状态的先验信息,可能是其他传感器得到的信息,现在想融合相机信息,使用 Bayes 法则,求出后验概率 [公式] 。

 

 

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

 

从零手写VIO——(四)基于滑动窗口算法的 VIO 系统:可观性和一致性(上)舒尔补插图(10)
(4)

即对等式右侧取对数 [公式] ,根据对数的性质拆开,求其负值的最小值。这里取对数的原因也很简单,高斯分布中有 [公式] (高斯分布高维形式: [公式] ),舍去系数、负负为正也就有了(7)式的形式:

 

5

(5)

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

从零手写VIO——(四)基于滑动窗口算法的 VIO 系统:可观性和一致性(上)舒尔补插图(15)
(6)

故有:

从零手写VIO——(四)基于滑动窗口算法的 VIO 系统:可观性和一致性(上)舒尔补插图(16)
(7)

此最小二乘的求解可以使用增量方程:(下式应该很好理解, [公式] 是系统状态量的增量,类比于经典方程的 [公式] ; [公式] 是信息矩阵,也是协方差的逆)

6

(8)

多元高斯分布

多元高斯分布(The Multivariate normal distribution)​www.cnblogs.com图标

零均值的多元高斯分布: [公式] , [公式] 是协方差矩阵,协方差矩阵的逆记作 [公式] 。三维变量的协方差矩阵为:

7
(9)

若变量间相互独立,那么除主对角线以外元素都为 0。在上讲中用连续时间下的狄拉克函数 [公式] 描述相互独立的关系。(9)式中用 [公式] 为对应元素求期望。

从零手写VIO——(四)基于滑动窗口算法的 VIO 系统:可观性和一致性(上)舒尔补插图(28)

Examples[1]

Example 1

从零手写VIO——(四)基于滑动窗口算法的 VIO 系统:可观性和一致性(上)舒尔补插图(29)

根据此描述,写出协方差矩阵:

从零手写VIO——(四)基于滑动窗口算法的 VIO 系统:可观性和一致性(上)舒尔补插图(30)
(10)

其中:

从零手写VIO——(四)基于滑动窗口算法的 VIO 系统:可观性和一致性(上)舒尔补插图(31)
(11)

此外[公式] , [公式] ,下面计算非对角元素:

从零手写VIO——(四)基于滑动窗口算法的 VIO 系统:可观性和一致性(上)舒尔补插图(34)
(12)

公式可以通过方差的性质直接写出: [公式] 。

上述这部分在论文中是这样书写的:(K就是 [公式] 协方差)

从零手写VIO——(四)基于滑动窗口算法的 VIO 系统:可观性和一致性(上)舒尔补插图(36)
从零手写VIO——(四)基于滑动窗口算法的 VIO 系统:可观性和一致性(上)舒尔补插图(37)

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

从零手写VIO——(四)基于滑动窗口算法的 VIO 系统:可观性和一致性(上)舒尔补插图(38)

推导应该是很容易理解的, [公式] ,且 [公式] 是 [公式] 的条件。然后就有个上面的式子,再经通分后写成矩阵形式就有了 [公式] 的形式,式中的 [公式] 就是协方差矩阵的逆,也称作信息矩阵。可以注意到,它的 [公式] 位置是0,表示 [公式] 关于 [公式] 条件独立。

若室内外温度正相关 [公式] ( [公式] ):

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

Example 2

从零手写VIO——(四)基于滑动窗口算法的 VIO 系统:可观性和一致性(上)舒尔补插图(51)

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

从零手写VIO——(四)基于滑动窗口算法的 VIO 系统:可观性和一致性(上)舒尔补插图(52)
(13)
从零手写VIO——(四)基于滑动窗口算法的 VIO 系统:可观性和一致性(上)舒尔补插图(53)
(14)

论文中是这样↓的:

从零手写VIO——(四)基于滑动窗口算法的 VIO 系统:可观性和一致性(上)舒尔补插图(54)

但是论文对逆矩阵 [公式] 的推导中有一个小的错误就是提取出 [公式] 后,位于矩阵 [公式]两个位置的分母并没有将 2 提出来,其他的没有问题,可以自己简单推一下。

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

从 Example 1 去除变量 [公式]

从零手写VIO——(四)基于滑动窗口算法的 VIO 系统:可观性和一致性(上)舒尔补插图(63)

由于公式是这样的:

从零手写VIO——(四)基于滑动窗口算法的 VIO 系统:可观性和一致性(上)舒尔补插图(64)
(15)

[公式] 和 [公式] 的取值都与 [公式] 无关,故可以直接在协方差矩阵上把有关于 [公式] 的全部删去:

16

(16)

矩阵对称的不要忘记!这样就变成了一个 [公式] 的矩阵。对于信息矩阵:

从零手写VIO——(四)基于滑动窗口算法的 VIO 系统:可观性和一致性(上)舒尔补插图(67)
(17)

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


舒尔补应用:边际概率,条件概率

舒尔补[2]定义

给定任意的分块矩阵 [公式] ,如下所示:

从零手写VIO——(四)基于滑动窗口算法的 VIO 系统:可观性和一致性(上)舒尔补插图(69)
(18)
  • 如果,矩阵块 [公式] 是可逆的,则 [公式] 称之为 [公式] 关于 [公式] 的舒尔补。
  • 如果,矩阵块 [公式] 是可逆的,则 [公式] 称之为 [公式] 关于 [公式] 的舒尔补。

Schur complement​en.wikipedia.org

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

如何得到舒尔补的形式

将 [公式] 矩阵变成上三角或者下三角形过程中,就会得到舒尔补:

从零手写VIO——(四)基于滑动窗口算法的 VIO 系统:可观性和一致性(上)舒尔补插图(74)
(19)

其中: [公式] 。联合起来,将 [公式] 变成对角矩阵:

从零手写VIO——(四)基于滑动窗口算法的 VIO 系统:可观性和一致性(上)舒尔补插图(76)
(20)

反过来还能恢复成矩阵 [公式] :

从零手写VIO——(四)基于滑动窗口算法的 VIO 系统:可观性和一致性(上)舒尔补插图(77)
(21)
从零手写VIO——(四)基于滑动窗口算法的 VIO 系统:可观性和一致性(上)舒尔补插图(78)
(22)

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

设多元变量 [公式] 服从高斯分布,且由两部分组成: [公式] ,变量构成的协方差矩阵:

从零手写VIO——(四)基于滑动窗口算法的 VIO 系统:可观性和一致性(上)舒尔补插图(81)

其中 [公式] 概率分布为:

23
从零手写VIO——(四)基于滑动窗口算法的 VIO 系统:可观性和一致性(上)舒尔补插图(84)
(23)

[公式] 是边际概率, [公式] 是条件概率。

从上式可知: [公式] 。即边际概率的协方差直接取矩阵块就成,条件概率的协方差是 [公式] 对应的舒尔补。

从上式(23)可以得到信息矩阵 [公式] :

从零手写VIO——(四)基于滑动窗口算法的 VIO 系统:可观性和一致性(上)舒尔补插图(90)
(24)

可以总结出 [公式] 的信息矩阵:

  • 条件概率 [公式] 的信息矩阵为: [公式] 。
  • 边际概率 [公式] 的信息矩阵为: [公式] 。 [公式] 是边际概率的信息矩阵, [公式] 是联合信息矩阵的部分矩阵块。

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

从零手写VIO——(四)基于滑动窗口算法的 VIO 系统:可观性和一致性(上)舒尔补插图(98)

从联合分布 [公式] 中边缘化掉变量 [公式] ,即 [公式] 对应的信息矩阵为:

从零手写VIO——(四)基于滑动窗口算法的 VIO 系统:可观性和一致性(上)舒尔补插图(101)
(25)

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

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

从零手写VIO——(四)基于滑动窗口算法的 VIO 系统:可观性和一致性(上)舒尔补插图(102)

参考

  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

发表评论

后才能评论