VSLAM：：[手写VIO_课堂笔记]第四讲(上)_基于滑动窗口算法的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}$

3. 最终要保留的变量$(x_1,x_2)$所对应的原信息矩阵子块就是$\Lambda_{aa}$, 消掉某个变量($x_3$)之后, 变量$(x_1,x_2)$最终的信息矩阵由式(38)来计算.

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

---

作业

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

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

(不知道是不是这样做?)