VSLAM：：[手写VIO_课堂笔记]第四讲(上)_基于滑动窗口算法的VIO系统原理

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

1.1. 高斯分布到信息矩阵

1.1.1. SLAM问题的模型

1.1.2. 举例

• 上面省略了一些步骤
  $$\begin{array}{r}\sum _{11}=Conv(x_1,x_1)=E([x_1-E(x_1){]}^2)\end{array}$$

又因为:
$$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{array}{rl}\sum _{11}=Conv(x_1,x_1)& =E([x_1-E(x_1){]}^2)\\ & =E([x_1])\\ & =w_1^{2}E(v_2^2)+2w_{1}E(v_1{v}_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{array}$$

下面是关于期望\方差\协方差的一些回顾:

协方差矩阵

以上参考自:

• 期望、方差计算
• 协方差矩阵介绍

注意:

• 上面的$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$是正相关的.

注意:

• 上面的$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=\left[\begin{array}{cc}A& C^T\\ C& D\end{array}\right]$$
3. 对应的关于条件概率$P(b|a)$则服从均值为$A^{-1}{C}^{T}b$, 方差为变量a的舒尔补${\Delta }_A$的正态分布.

解释:

1. 就是说,在SLAM问题里面,我们直接操作的只有多元变量$x=[a,b{]}^T$的信息矩阵(注意,是信息矩阵,不是协方差矩阵)
   $$K^{-1}={\left[\begin{array}{cc}A& C^T\\ C& D\end{array}\right]}^{-1}$$
2. 根据公式(29), 即我们只有这样一个形式的信息矩阵:
   $$\begin{array}{rl}{K}^{-1}& =\left[\begin{array}{cc}{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{array}\right]\end{array}$$
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{array}{r}P(x_1,x_2,x_3)=P(x_1,x_2)P(x_3|x_1,x_2)\end{array}$$

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之后的信息矩阵

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