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}$11∑​=Conv(x1​,x1​)=E([x1​−E(x1​)]2)​

又因为:
$E(x_1)=E(w_1 v_2+v_1)=w_1 E(v_2)+E(v_1)$E(x1​)=E(w1​v2​+v1​)=w1​E(v2​)+E(v1​)
且$v_1$v1​和$v_2$v2​都是0均值正态分布,所以
$E(x_1)=E(w_1 v_2+v_1)=w_1 E(v_2)+E(v_1)=0$E(x1​)=E(w1​v2​+v1​)=w1​E(v2​)+E(v1​)=0
所以:
$\sum_{11}=Conv(x_1,x_1)=E([x_1-E(x_1)]^2)=E([x_1]^2)$11∑​=Conv(x1​,x1​)=E([x1​−E(x1​)]2)=E([x1​]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}$11∑​=Conv(x1​,x1​)​=E([x1​−E(x1​)]2)=E([x1​])=w12​E(v22​)+2w1​E(v1​v2​)+E(v12​)=w12​[E(v22​)−E(v2​)2]+0+[E(v12​)−E(v1​)2]=w12​σ22​+σ12​​

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

协方差矩阵

以上参考自:

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

一些特性

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

---

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

---

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

解释:

1. 对$x=[a,b]^T$x=[a,b]T里面的变量$a$a进行边际概率, 即把$a$a看做是固定(就是边缘化操作)的时候, $a$a的分布正比于$\exp(-\frac{1}{2}a^T A^{-1} a)$exp(−21​aTA−1a), 也就是服从均值为0, 方差为A的正态分布
2. 此时的边际概率$P(a)$P(a)的协方差就是多元变量x的协方差矩阵$K$K中的矩阵块, 在这个例子中就是矩阵块$A$A
   $K= \begin{bmatrix} A & C^T \\ C & D \end{bmatrix}$K=[AC​CTD​]
3. 对应的关于条件概率$P(b|a)$P(b∣a)则服从均值为$A^{-1}C^Tb$A−1CTb, 方差为变量a的舒尔补$\Delta_A$ΔA​的正态分布.

解释:

1. 就是说,在SLAM问题里面,我们直接操作的只有多元变量$x=[a,b]^T$x=[a,b]T的信息矩阵(注意,是信息矩阵,不是协方差矩阵)
   $K^{-1}= \begin{bmatrix} A & C^T \\ C & D \end{bmatrix} ^{-1}$K−1=[AC​CTD​]−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}$K−1​=[A−1+A−1CTΔA−1​CA−1−ΔA−1​CA−1​−A−1CTΔA−1​ΔA−1​​]​
3. 需要从上面形式的信息矩阵中恢复出变量$a$a的信息矩阵,即矩阵$A^{-1}$A−1,则可利用公式(38)

解释:

1. 回顾例子(1),有3个变量$x_1,x_2,x_3$x1​,x2​,x3​, 如果去掉变量$x_3$x3​, 对应的信息矩阵就是原来信息矩阵中关于变量$x_3$x3​的项全部消掉, 由于实际操作中并没有颜色标记, 所以应用了舒尔补公式来进行这个消掉$x_3$x3​相关项的操作.
2. 具体操作就是:把$(x_1,x_2)$(x1​,x2​)看做是上面多元高斯分布$x=[a,b]^T$x=[a,b]T里面的$a$a, 把$x_3$x3​看做是$b$b, 可以看到, 当$x_1,x_2$x1​,x2​(也就是a)被边缘化, 边缘化之后的分布(即边际概率)$P(x_1,x_2)$P(x1​,x2​)对应的信息矩阵可以用公式(38)得到.
   $\begin{aligned} P(x_1,x_2,x_3)=P(x_1,x_2)P(x_3|x_1,x_2) \end{aligned}$P(x1​,x2​,x3​)=P(x1​,x2​)P(x3​∣x1​,x2​)​
3. 所以说, 要消掉某个变量(如$x_3$x3​), 则是将其他另外的变量(如$x_1,x_2$x1​,x2​)边缘化.
4. 最终要保留的变量$(x_1,x_2)$(x1​,x2​)所对应的原信息矩阵子块就是$\Lambda_{aa}$Λaa​, 消掉某个变量($x_3$x3​)之后, 变量$(x_1,x_2)$(x1​,x2​)最终的信息矩阵由式(38)来计算.
5. 换句话说: 当最终要保留的变量是$(x_2,x_3)$(x2​,x3​), 而要消掉的变量是$x_1$x1​时, 变量$(x_2,x_3)$(x2​,x3​)在原信息矩阵$K^{-1}$K−1所对应的矩阵子块为右下角的部分, 即原信息矩阵的右下角子块才是$\Lambda_{aa}$Λaa​
   

---

作业

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

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

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