VIO第四讲

2020-02-14

Source

Edit

History

第四讲(上)_基于滑动窗口算法的VIO系统原理

Catalogue

1. 1. 第四讲(上)_基于滑动窗口算法的VIO系统原理

1. 第四讲(上)_基于滑动窗口算法的VIO系统原理

1.1. 高斯分布到信息矩阵

1.1.1. SLAM问题的模型

1.1.2. 举例

上面省略了一些步骤 \[ \begin{aligned} \sum_{11}=Conv(x_1,x_1)=E([x_1-E(x_1)]^2) \end{aligned} \]

又因为: \[ E(x_1)=E(w_1 v_2+v_1)=w_1 E(v_2)+E(v_1) \] 且\(v_1\)和\(v_2\)都是0均值正态分布,所以 \[E(x_1)=E(w_1 v_2+v_1)=w_1 E(v_2)+E(v_1)=0\] 所以: \[\sum_{11}=Conv(x_1,x_1)=E([x_1-E(x_1)]^2)=E([x_1]^2)\] 最终: \[ \begin{aligned} \sum_{11}=Conv(x_1,x_1)&=E([x_1-E(x_1)]^2) \\ &=E([x_1]) \\ &=w_1^2E(v_2^2)+2w_1E(v_1v_2)+E(v_1^2) \\ &=w_1^2[E(v_2^2)-E(v_2)^2]+0+[E(v_1^2)-E(v_1)^2] \\ &=w_1^2\sigma_2^2+\sigma_1^2 \end{aligned} \]

下面是关于期望: 协方差矩阵

以上参考自: * 期望、方差计算 * 协方差矩阵介绍

注意: * 上面的\(\color{red}{p(x_1,x_2,x_3)=p(x_2)p(x_1|x_2)p(x_3|x_2)}\)是与给出的例子结合起来的, 例子是有室外的温度(即变量\(x_2\)), 而房间1和房间3的温度是分别仅与室外温度\(x_2\)相关.

一些特性 1. 如果协方差矩阵中,非对角元素\(\sum_{ij}>0\)表示两个变量正相关. 2. 在信息矩阵(协方差矩阵的逆)中,对应的非对角元素\(\sum_{ij}<0\)或\(\sum_{ij}=0\). 如\(\Lambda_{12}<0\)则表示在变量\(x_3\)发生(确定)的条件下,元素\(x_1\)和\(x_2\)是正相关的.

注意: * 上面的\(\color{red}{p(x_1,x_2,x_3)=p(x_1)p(x_3)p(x_2|x_1,x_2)}\)是与给出的例子2结合起来的, 例子2是变量\(x_2\)由\(x_1,x_3\)共同给出

注意: * 去除一个变量, 实际上就是将该变量所携带的信息转化为剩余变量的先验 -------------------------------------------------

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

这里可以往回看一下多元高斯分布,上几页ppt

解释: 1. 对\(x=[a,b]^T\)里面的变量\(a\)进行边际概率, 即把变量\(b\)去掉的时候, \(a\)的分布正比于\(\exp(-\frac{1}{2}a^T A^{-1} a)\), 也就是服从均值为0, 方差为A的正态分布 2. 此时的边际概率\(P(a)\)的协方差就是多元变量x的协方差矩阵\(K\)中的矩阵块, 在这个例子中就是矩阵块\(A\) \[ K= \begin{bmatrix} A & C^T \\ C & D \end{bmatrix} \] 3. 对应的关于条件概率\(P(b|a)\)则服从均值为\(A^{-1}C^Tb\), 方差为变量a的舒尔补\(\Delta_A\)的正态分布.

解释: 1. 就是说,在SLAM问题里面,我们直接操作的只有多元变量\(x=[a,b]^T\)的信息矩阵(注意,是信息矩阵,不是协方差矩阵) \[ K^{-1}= \begin{bmatrix} A & C^T \\ C & D \end{bmatrix} ^{-1} \] 2. 根据公式(29), 即我们只有这样一个形式的信息矩阵: \[ \begin{aligned} K^{-1} &= \begin{bmatrix} A^{-1}+A^{-1}C^T \Delta_{A}^{-1} C A^{-1} & -A^{-1}C^T\Delta_{A}^{-1} \\ -\Delta_{A}^{-1} C A^{-1} & \Delta_{A}^{-1} \end{bmatrix} \end{aligned} \] 3. 需要从上面形式的信息矩阵中恢复出变量\(a\)的信息矩阵,即矩阵\(A^{-1}\),则可利用公式(38)

注意: 1. 对某个多元高斯分布\(P(a,b)\)进行分解,可分解为: * \(P(a,b)=P(a|b)P(a)\): 这种情况就是不再关注变量b, 而边际分布\(P(a)\)的信息矩阵包含了就是把变量\(b\)所携带的信息, 就是说此时\(P(a)\)分布是包含了变量\(b\)信息的先验. (适用于去掉变量b) * \(P(a,b)=P(b|a)P(b)\): 这种情况就是不再关注变量a, 二边际分布\(P(b)\)的信息矩阵包含了就是把变量\(a\)所携带的信息, 就是说此时\(P(b)\)分布是包含了变量\(a\)信息的先验. (适用于去掉变量a)

解释: 1. 回顾例子(1),有3个变量\(x_1,x_2,x_3\), 如果去掉变量\(x_3\), 对应的信息矩阵就是原来信息矩阵中关于变量\(x_3\)的项全部消掉, 由于实际操作中并没有颜色标记, 所以应用了舒尔补公式来进行这个消掉\(x_3\)相关项的操作. 2. 具体操作就是:把\((x_1,x_2)\)看做是上面多元高斯分布\(x=[a,b]^T\)里面的\(a\), 把\(x_3\)看做是\(b\), 可以看到, b边缘化之后的分布(即边际概率)\(P(x_1,x_2)\)对应的信息矩阵可以用公式(38)得到. \[ \begin{aligned} P(x_1,x_2,x_3)=P(x_1,x_2)P(x_3|x_1,x_2) \end{aligned} \]

最终要保留的变量\((x_1,x_2)\)所对应的原信息矩阵子块就是\(\Lambda_{aa}\), 消掉某个变量(\(x_3\))之后, 变量\((x_1,x_2)\)最终的信息矩阵由式(38)来计算.
换句话说: 当最终要保留的变量是\((x_2,x_3)\), 而要消掉的变量是\(x_1\)时, 变量\((x_2,x_3)\)在原信息矩阵\(K^{-1}\)所对应的矩阵子块为右下角的部分, 即原信息矩阵的右下角子块才是\(\Lambda_{aa}\)

作业

1. 画出相机变量\(\xi_1\)被marg之后的信息矩阵

2. 画出相机变量\(\xi_2\)被marg之后的信息矩阵