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之後的信息矩陣

(不知道是不是這樣做?)