ytwy

SLAM學習筆記—後端

1. 概述

   1. 狀態估計概率分佈的核心思想

      未知量(\(x_k\))的後驗概率分佈 = 似然概率分佈 × 未知量(\(x_k\))的先驗概率分佈

      這一等式貫穿全文，請牢牢抓住！

   2. 運動方程和觀測方程

      \[\begin{cases} x_k = f(x_{k-1},u_k)+w_k \\\\ z_k=h(x_k)+v_k \end{cases} ，k=1,2,\cdots,N \]

      其中\(x_k\overset{def}{=}\{x_k,y_1\cdots,y_m\}\)，表示\(k\)時刻所有的未知量（觀測位姿，路標位置）

      1. 運動方程用來估計未知量的先驗（預測）：先估計出一個先驗\(\hat{x}_k\)；
      2. 觀測方程用來估計未知量的後驗（更新）：觀測先驗值後消除其不確定性，得出確切數值\(x_k\)，然後利用似然概率估計觀測值\(z_k\)，最後利用\(z_k\)校正\(x_k\)得到後者的後驗分佈\(\check{x}_k\)；

      可以看到左圖，如果對未知量\(x_k\)做先驗估計，由於誤差的積累，系統噪音會越來越大。

      但是如果通過觀測路標點來矯正未知量，對其做後驗估計，則誤差不會一直增加。

   3. 線性系統和KF

      就是運動方程和觀測方程均可以線性表出。

2. 狀態估計概率分佈（貝葉斯法則在運動方程和觀測方程中的應用）

   1. \(k\)時刻未知量的後驗概率分佈記爲\(P(\check{x}_k)\)，其實際含義爲：在已知\(x_0,u_{1:k},z_{1:k}\)的情況下，\(x_k\)的後驗概率分佈。所以也可以記爲\(P(x_k|x_0,u_{1:k},z_{1:k})\)；
   2. \(k\)時刻未知量的先驗概率分佈記爲\(P(\hat{x}_k)\)，其實際 含義爲：在未知\(z_k\)（也就是隻已知\(x_0,u_{1:k},z_{1:k-1}\)）的情況下，\(x_k\)的先驗概率分佈。所以也可以記爲\(P(x_k|x_0,u_{1:k},z_{1:k-1})\)；
   3. \(k\)時刻的似然概率分佈記爲\(P(z_k)\)，其實際含義爲：在已知未知量的先驗值的基礎上，觀測值\(z_k\)的概率分佈。所以也可以記爲\(P(z_k|x_k)\)；

   所以，利用貝葉斯法則有：\(P(x_k|x_0,u_{1:k},z_{1:k})\varpropto P(z_k|x_k)·P(x_k|x_0,u_{1:k},z_{1:k-1})\)；

3. 線性系統和\(\mbox{KF(Kalman Filter)}\)【非主流】

   我們假設當前狀態\(x_k\)只與其前一刻的狀態相關，也就是說我們將先驗概率分佈函數按照\(x_{k-1}\)全概展開：

   \[\begin{aligned} P(B)&=\sum_{i=1}^{N}P(B|A_i)·P(A_i)，其中B爲\hat{x}_k，A爲\check{x}_{k-1} \\ P(\hat{x}_k|x_0,u_{1:k},z_{1:k-1})&=\int P(\hat{x}_k|\check{x}_{k-1},x_0,u_{1:k},z_{1:k-1})·P(\check{x}_{k-1}|x_0,u_{1:k},z_{1:k-1})dx_{k-1} \end{aligned} \]

   按照線性假設，我們可以去除掉等式中的無關變量（化簡等式）：

   1. 對於條件概率（已知\(k-1\)時刻的後驗和\(k\)時刻的輸入值）：由於\(\check{x}_{k-1}\)已經包含了\(u_{1:k-1}\)和\(z_{1:k-1}\)，所以可以簡化爲：\(P(\hat{x}_k|\check{x}_{k-1},u_k)\)；
   2. 對於\(k-1\)時刻後驗概率：\(u_k\)不會影響到\(k-1\)的後驗，即可化簡爲：\(P(\check{x}_{k-1}|x_0,u_{1:k-1},z_{1:k-1})\)；

   馬爾科夫鏈即爲：\(P(\hat{x}_k|x_0,u_{1:k},z_{1:k-1},z_{k})\varpropto P(z_k|\hat{x}_k)·\int{P(\hat{x}_k|\check{x}_{k-1},u_k)}P(\check{x}_{k-1}|x_0,u_{1:k-1},z_{1:k-1})dx_{k-1}\)；

   由於是正比關係，所以我們可以假設運動方程和觀測方程均可服從線性表出：

   \[\begin{cases} \hat{x}_k=A_k\check{x}_{k-1}+u_k+w_k \\\\ z_k=C_k\hat{x}_k+v_k \end{cases} ，其中噪聲服從高斯分佈: \begin{cases} w_k\sim N(0,R_k) \\\\ u_k\sim N(0,Q_k) \end{cases} \]

   1. 記\(k\)時刻\(x_k\)的先驗概率分佈爲\(\hat{X}_k\sim N(\hat{\mu}_k,\hat{\Sigma}_k)\)、後驗概率分佈爲\(\check{X}_k\sim N(\check{\mu}_k,\check{\Sigma}_k)\)；
   2. 由於在觀測時必須基於先驗數據，所以此時的先驗數據將會坍塌，不確定性隨之消失，也就是說此時的未知量\(\hat{x}_k=x_k\sim N(x_k,0)\)（是一個常向量），所以可記\(k\)時刻似然概率分佈爲\(Z_k\sim N(C_kx_k,Q_k)\)；

   由隨機變量函數的概率分佈函數計算公式，可以得出：

   \[\begin{cases} \hat{\mu}_k=A_k\check{\mu}_{k-1}+u_k \\ \hat{\Sigma}_k=A_k\check{\Sigma}_{k-1}A_k^T+R_k \end{cases} \]

   然後由於都是服從高斯分佈，所以我們可以只對比指數即可（指數就是馬氏距離，類似於二次型），對於先驗後驗的位姿參數\(x_k\)不做符號上的區分，因爲已經在均值向量和協方差矩陣上體現出了分佈差異，且本質上都是對位姿參數的分佈解釋，故統一用符號\(x_k\)表示。

   \[\begin{aligned} 後驗概率分佈：&{(x_k-\check{\mu}_k)}^T·\check{\Sigma}_k^{-1}·{(x_k-\check{\mu}_k)} \\ 似然概率分佈：&{(z_k-C_kx_k)}^T·Q_k^{-1}·{(z_k-C_kx_k)} \\ 先驗概率分佈：&{(x_k-\hat{x}_k)}^T·\hat{\Sigma}_k^{-1}·{(x_k-\hat{x}_k)} \end{aligned} \]

   在指數層面觀察，後驗概率分佈 = 似然概率分佈 + 先驗概率分佈，即有如下等式：

   \[\matrix{ {(x_k-\check{\mu}_k)}^T·\check{\Sigma}_k^{-1}·{(x_k-\check{\mu}_k)} &={(z_k-C_kx_k)}^T·Q_k^{-1}·{(z_k-C_kx_k)}+{(x_k-\hat{\mu}_k)}^T·\hat{\Sigma}_k^{-1}·{(x_k-\hat{\mu}_k)} \\ {(x_k^T-\check{\mu}_k^T)}·\check{\Sigma}_k^{-1}·{(x_k-\check{\mu}_k)}&={(z_k^T-x_k^TC_k^T)}·Q_k^{-1}·{(z_k-C_kx_k)}+{(x_k^T-\hat{\mu}_k^T)}·\hat{\Sigma}_k^{-1}·{(x_k-\hat{\mu}_k)} } \]

   \[\begin{aligned} 等是左邊=&{(x_k^T-\check{\mu}_k^T)}·\check{\Sigma}_k^{-1}·{(x_k-\check{\mu}_k)} \\=& \underbrace{x_k^T·\check{\Sigma}_k^{-1}·x_k}_{二次項} \underbrace{-x_k^T·\check{\Sigma}_k^{-1}·\check{\mu}_k-\check{\mu}_k^T·\check{\Sigma}_k^{-1}·x_k}_{一次項} \underbrace{+\check{\mu}_k^T·\check{\Sigma}_k^{-1}·\check{\mu}_k}_{常數項} \\ 等式右邊=&{(z_k^T-x_k^TC_k^T)}·Q_k^{-1}·{(z_k-C_kx_k)}+{(x_k^T-\hat{\mu}_k^T)}·\hat{\Sigma}_k^{-1}·{(x_k-\hat{\mu}_k)} \\ =& z_k^T·Q_k^{-1}·z_k -z_k^T·Q_k^{-1}·C_kx_k -x_k^TC_k^T·Q_k^{-1}·z_k +x_k^TC_k^T·Q_k^{-1}·C_kx_k \\& +x_k^T·\hat{\Sigma}_k^{-1}·x_k -x_k^T·\hat{\Sigma}_k^{-1}·\hat{\mu}_k -\hat{\mu}_k^T·\hat{\Sigma}_k^{-1}·x_k +\hat{\mu}_k^T·\hat{\Sigma}_k^{-1}·\hat{\mu}_k \\ =& \underbrace{+x_k^TC_k^T·Q_k^{-1}·C_kx_k+x_k^T·\hat{\Sigma}_k^{-1}·x_k}_{二次項} \\ & \underbrace{-z_k^T·Q_k^{-1}·C_kx_k-x_k^TC_k^T·Q_k^{-1}·z_k-x_k^T·\hat{\Sigma}_k^{-1}·\hat{\mu}_k-\hat{\mu}_k^T·\hat{\Sigma}_k^{-1}·x_k}_{一次項} \\ & \underbrace{z_k^T·Q_k^{-1}·z_k+\hat{\mu}_k^T·\hat{\Sigma}_k^{-1}·\hat{\mu}_k}_{常數項} \\=& \underbrace{x_k^T·{(C_k^TQ_k^{-1}C_k+\hat{\Sigma}_k^{-1})}·x_k}_{二次項} \\& \underbrace{-[(z_k^TQ_k^{-1}C_k+\hat{\mu}_k^T\hat{\Sigma}_k^{-1})·x_k+x_k^T·(C_k^TQ_k^{-1}z_k+\hat{\Sigma}_k^{-1}\hat{\mu}_k)]}_{一次項} \\& \underbrace{z_k^T·Q_k^{-1}·z_k+\hat{\mu}_k^T·\hat{\Sigma}_k^{-1}·\hat{\mu}_k}_{常數項} \end{aligned} \]

   所以對應冪次相等原則，可得如下等式：

   \[\begin{cases} 二次項：&\check{\Sigma}_k^{-1}=C_k^TQ_k^{-1}C_k+\hat{\Sigma}_k^{-1} \\\\ 一次項：& \small{-x_k^T\check{\Sigma}_k^{-1}\check{\mu}_k-\check{\mu}_k^T\check{\Sigma}_k^{-1}x_k = -x_k^T(C_k^TQ_k^{-1}z_k+\hat{\Sigma}_k^{-1}\hat{\mu}_k) -(z_k^TQ_k^{-1}C_k+\hat{\mu}_k^T\hat{\Sigma}_k^{-1})x_k} \\\\ 常數項：&\check{\mu}_k^T\check{\Sigma}_k^{-1}\check{\mu}_k=z_k^TQ_k^{-1}z_k+\hat{\mu}_k^T\hat{\Sigma}_k^{-1}\hat{\mu}_k \end{cases} \]

   1. 比較二次項係數得後驗協方差與先驗協方差之間的關係

      1. 等式兩邊同時左乘\(\check{\Sigma}_k\)：\(I=\check{\Sigma}_kC_k^TQ_k^{-1}C_k+\check{\Sigma}_k\hat{\Sigma}_k^{-1}\)；
      2. 代入卡爾曼增益：\(I=KC_k+\check{\Sigma}_k\hat{\Sigma}_k^{-1}\)；
      3. 整理得協方差關係：\(\check{\Sigma}_k=(I-KC_k)\hat{\Sigma}_k\)；

      【注】可利用\(\mbox{SWM(Sherman-Morrison-Woodbury)}\)恆等式解出\(K\)，該方法可破解循環定義問題。

      \[\begin{aligned} &K=\check{\Sigma}_kC_k^TQ_k^{-1}=(C_k^TQ_k^{-1}C_k+\hat{\Sigma}_k^{-1})^{-1}C_k^TQ_k^{-1}\\ &由SMW恆等式：AB{(D+CAB)}^{-1}\equiv {(A^{-1}+BD^{-1}C)}^{-1}BD^{-1}\\ &令A=\hat{\Sigma}_k,B=C_k^T,C=C_k,D=Q_k，可得：\\ &K=(C_k^TQ_k^{-1}C_k+\hat{\Sigma}_k^{-1})^{-1}C_k^TQ_k^{-1}=\hat{\Sigma}_kC_k^T{(Q_k+C_k\hat{\Sigma}_kC_k^T)}^{-1}\\ &即證：K=\check{\Sigma}_kC_k^TQ_k^{-1}\equiv \hat{\Sigma}_kC_k^T{(Q_k+C_k\hat{\Sigma}_kC_k^T)}^{-1}，卡爾曼增益可由先驗算得 \end{aligned} \]

   2. 比較一次項係數得後驗均值與先驗均值之間的關係

      1. 常數和協方差的轉置都是本身：\(-2\check{\mu}_k^T\check{\Sigma}_k^{-1}x_k=-2(z_k^TQ_k^{-1}C_k+\hat{\mu}_k^T\hat{\Sigma}_k^{-1})x_k\)；
      2. 取\(x_k\)的係數並轉置：\(\check{\Sigma}_k^{-1}\check{\mu}_k=C_k^TQ_k^{-1}z_k+\hat{\Sigma}_k^{-1}\hat{\mu}_k\)；
      3. 等式左右兩邊同時左乘\(\check{\Sigma}_k\)：\(\check{\mu}_k=\check{\Sigma}_kC_k^TQ_k^{-1}z_k+\check{\Sigma}_k\hat{\Sigma}_k^{-1}\hat{\mu}_k\)；
      4. 代入卡爾曼增益和協方差關係：\(\check{\mu}_k=\hat{\mu}_k+K(z_k-C_k\hat{\mu}_k)=\hat{\mu}_k+Kv_k\)；

      【注】所以該關係的意義就是“後驗均值”爲“先驗均值”加上一個修正量，而這個修正量就是對觀測噪音（誤差）做一個卡爾曼增益旋轉（左乘爲向量旋轉，右乘爲座標系旋轉）。

   總結一下，我們通過定義了一個叫“卡爾曼增益”的中間變量建立起了（1）協方差矩陣的先後驗概率關係（2）均值的先後驗概率關係。於是我們可以將“後驗 = 似然 × 先驗”的過程分解爲兩個步驟：

   1. 預測（通過前一時刻的後驗估計當前時刻的先驗）

      \[\begin{cases} \hat{\mu}_k=A_k\check{\mu}_{k-1}+u_k \\ \hat{\Sigma}_k=A_k\check{\Sigma}_{k-1}A_k^T+R \end{cases} \]

   2. 更新（通過卡爾曼增益更新先驗從而得到後驗）

      \[\begin{cases} \check{\mu}_k=\hat{\mu}_k+K(z_k-C_k\hat{\mu}_k) \\ \check{\Sigma}_k=(I-KC_k)\hat{\Sigma}_k \end{cases} \]

4. 非線性系統和\(\mbox{EKF(Extended Kalman Filter)}\)【對KF擴展至一階】

   推到過程與\(\mbox{KF}\)類似，只是核心思想有些微差異。\(\mbox{EKF}\)是對\(\mbox{KF}\)在\(\check{x}_{k-1}=\check{\mu}_{k-1}\)處和\(\hat{x}_k=\hat{\mu}_k\)的一階泰勒展開：

   \[\begin{aligned} &\begin{cases} \hat{x}_k\approx f(\check{\mu}_{k-1},u_k)+\frac{\part{f}}{\part{\check{x}_{k-1}}}|_{\check{x}_{k-1}=\check{\mu}_{k-1}}·(\check{x}_{k-1}-\check{\mu}_{k-1})+w_k \\ z_k\approx h(\hat{\mu}_k)+\frac{\part{h}}{\part{\hat{x}_k}}|_{\hat{x}_k=\hat{\mu}_k}·(\hat{x}_k-\hat{\mu}_k)+v_k \end{cases} \\\\ &記F= \frac{\part{f}}{\part{\check{x}_{k-1}}}|_{\check{x}_{k-1}=\check{\mu}_{k-1}}，H=\frac{\part{h}}{\part{\hat{x}_k}}|_{\hat{x}_k=\hat{\mu}_k} \\\\ &\begin{cases} \hat{x}_k\approx F\underbrace{(\check{x}_{k-1}-\check{\mu}_{k-1})}_{\sim N(0,\check{\Sigma}_{k-1})}+\underbrace{w_k}_{\sim N(0,R)}+\underbrace{f(\check{\mu}_{k-1},u_k)}_{常數項} &\sim N(f(\check{\mu}_{k-1},u_k)，F\check{\Sigma}_{k-1}F^T+R_k) \\\\ z_k\approx H\underbrace{(\hat{x}_k-\hat{\mu}_k)}_{\sim N(0,\hat{\Sigma}_k)}+\underbrace{v_k}_{\sim N(0,Q_k)}+\underbrace{h(\hat{\mu}_k)}_{常數項} &\sim N(h(\hat{\mu}_k)，H\hat{\Sigma}_kH^T+Q_k) \end{cases} \end{aligned} \]

   然後貝葉斯展開並對比冪次係數可得：

   \[\begin{cases} \check{\mu}_k=\hat{\mu}_k+K_k(z_k-h(\hat{\mu}_k)) \\\\ \check{\Sigma}_k=(I-K_kH)\hat{\Sigma}_k \end{cases} ，其中K_k爲卡爾曼增益\overset{def}{=}\hat{\Sigma}_kH^T{(Q_k+H\hat{\Sigma}_kH^T)}^{-1} \]