【機器視覺】4. 卡爾曼濾波

05-23

【機器視覺】4. 卡爾曼濾波

來自專欄 Let Machine See

上一節我們介紹了目標跟蹤中最常用的方法——光流法。

Calvin Shi：【機器視覺】3. 目標跟蹤：光流法?

zhuanlan.zhihu.com

目標跟蹤需要在包含雜訊的觀測序列中估計時變系統的狀態，這種估計稱為濾波估計。濾波估計中最經典的演算法就是卡爾曼濾波，本文對其原理進行介紹。

希望知道變數X在t時刻的值 $X_t$ ，可以有兩種方法。一種方法是，通過研究 $X_{t-1} ightarrow X_t$ 的變化規律，從初始值 $X_0$ 逐步遞推到 $X_t$ ，但是，遞推往往存在外界雜訊的干擾 $W_t$ ，對於零輸入線性系統有 $X_t=A_{t,t-1}X_{t-1}+W_t$ ；另一種方法是，引入一個測量設備測量 $X_t$ 的值，記為 $Z_t$ ，測量設備同樣存在雜訊 $V_t$ ，對於零輸入線性系統有 $Z_t=C_{t}X_{t}+V_t$ 。我們需要綜合這兩種方法確定一個估計結果，使得均方誤差最小。

對零輸入線性系統

$egin{cases} X_t=A_{t,t-1}X_{t-1}+W_t\ Z_t=C_tX_t+V_t end{cases}$ $X_t$ 是t時刻我們研究對象的狀態向量， $A_{t,t-1}$ 是一個矩陣，對 $X_{t-1}$ 進行線性變換。 $Z_t$ 是t時刻的觀測向量， $C_t$ 是增益矩陣。V是服從 $mathcal{N}(0,R)$ 的高斯白雜訊，W是服從 $mathcal{N}(0,Q)$ N(0,Q)的高斯白雜訊。我們希望對這個系統確定最優狀態估計 $hat X_t$ ，使得 $min |X-hat X|^2$

我們先考慮一種簡單的情形，如果我們有一個感測器A，A感測器測量結果是一個連續數值，且這個數值服從以下正態分布 $X_Asim mathcal{N}(mu_A,sigma_A^2)$ ；為了提高測量的質量我們可以進行多次測量然後取其平均值作為我們的最終測量結果，這樣可以減少誤差。假設進行兩次測量，我們有以下結果

$frac {X_{A1}+X_{A2}}{2} sim mathcal{N}(mu_A,frac{sigma_A^2}{2})$

但是有時候我們需要一個實時的測量結果——重複測量並不被允許，也不可能被實現。此時可以考慮再增加一個感測器，分別進行測量。然後，取兩個感測器的加權平均值來減少總體測量的誤差，權值設置的目的是要使得方差最小化。

$egin{cases} X_Asim mathcal{N}(mu_A,sigma_A^2)\X_Bsim mathcal{N}(mu_B,sigma_B^2)\ X_Asim mathcal{N}(mu_A,sigma_A^2)\X_Bsim mathcal{N}(mu_B,sigma_B^2) end{cases}$

最終結果

$egin{cases} hat X=kX_A+(1-k)X_B\ hat X sim mathcal{N}(kmu_A+(1-k)mu_B,k^2sigma^2_A+(1-k)^2sigma^2_B end{cases}$

即解優化問題

$egin{aligned} min & f(k)=k^2sigma^2_A+(1-k)^2sigma^2_B\ ext{s.t.} & k in (0,1) end{aligned}$

則

$frac{d}{dk}f(k)=2ksigma_A^2-2(1-k)sigma^2_B=0 Rightarrow k=frac{sigma^2_B}{sigma^2_A+sigma^2_B}$

於是

$egin{cases} hat X=frac{sigma^2_B}{sigma^2_A+sigma^2_B}X_A+frac{sigma^2_A}{sigma^2_A+sigma^2_B}X_B\hat X sim mathcal{N}(frac{sigma^2_B}{sigma^2_A+sigma^2_B}mu_A+frac{sigma^2_A}{sigma^2_A+sigma^2_B}mu_B,frac{sigma^2_Asigma^2_B}{sigma^2_A+sigma^2_B}) end{cases}$

方差

$S^2 (hat X)=sigma^2_A(1-frac{sigma^2_A}{sigma^2_A+sigma^2_B})$

下面我們用類似的方法討論對時間序列 $X_t$ 的最優狀態估計。我們首先來證明 $hat X_{t,t-1}=A_{t,t-1}hat X_{t-1}$ 是 $X_t$ 的一個無偏估計，設誤差 $e_t=hat X_{t-1}-X_{t-1}$ ，則

$egin{split} e_{t,t-1} &=hat X_{t,t-1}-X_t \ &=A_{t,t-1}hat X_{t-1}-A_{t,t-1}X_{t-1}-W\ &=A_{t,t-1}[hat X_{t-1}-X_{t-1}]-W end{split}$

所以

$egin{split} mathbb{E}[e_{t,t-1}] &=mathbb{E}[A_{t,t-1}(hat X_{t-1}-X_{t-1})-W] \ &=A_{t,t-1}mathbb{E}[hat X_{t-1}-X_{t-1}]-mathbb{E}[W]\ &=0 end{split}$

接著，我們定義均方誤差 $P_{t,t-1} overset{ m{def}}{=} mathbb{D}[e_{t,t-1}]$ ，那麼

$egin{split} mathbb{D}[e_{t,t-1}] &=mathbb{E}[(e_{t,t-1}-mathbb{E}[e_{t,t-1}])(e_{t,t-1}-mathbb{E}[e_{t,t-1}])^T]\ &=mathbb{E}[(e_{t,t-1}-0)(e_{t,t-1}-0)^T]\ &=mathbb{E}[e_{t,t-1}e_{t,t-1}^T]\ &=mathbb{E}[(A_{t,t-1}(hat X_{t-1}-X_{t-1})-W)(A_{t,t-1}(hat X_{t-1}-X_{t-1})-W)^T]\ &=mathbb{E}[A_{t,t-1}(hat X_{t-1}-X_{t-1})^2A_{t,t-1}^T-2A_{t,t-1}(hat X_{t-1}-X_{t-1})W^T+WW^T]\ &=A_{t,t-1}mathbb{E}[(hat X_{t-1}-X_{t-1})^2]A_{t,t-1}^T-2A_{t,t-1}mathbb{E}(hat X_{t-1}-X_{t-1})W^T+mathbb{E}[WW^T]\ &=A_{t,t-1}P_{t-1}A_{t,t-1}^T+Q end{split}$

即：

$P_{t,t-1}=A_{t,t-1}P_{t-1}A^T_{t,t-1}+Q$

然後，我們可以設想對於我們最後的估計結果一定是一個關於觀測向量與t?1時刻估計值的線性方差，即

$egin{split} hat X_t &=ahat X_{t-1}+bZ_t\ &=(A_{t,t-1}-bC_tA_{t,t-1})hat X_{t-1}+bZ_t\ &=A_{t,t-1}hat X_{t-1}+b(Z_t-C_tA_{t,t-1}hat X_{t-1})\ &=hat X_{t,t-1}+b(Z_t-C_that X_{t,t-1}) end{split}$

對於方差有

$egin{split} P_t &=mathbb{D}[e_t]\ &=mathbb{D}[hat X_{t,t-1}+b(Z_t-C_that X_{t,t-1})-X_t]\ &=mathbb{D}[(I-bC_t)hat X_{t,t-1}+b(C_tX_t+V)-X_t]\ &=mathbb{D}[(I-bC_t)hat X_{t,t-1}-(I-bC_t)X_t+bV]\ &=mathbb{D}[(I-bC_t)(hat X_{t,t-1}-X_t)+bV]\ &=(I-bC_t)P_{t,t-1}(I-bC_t)^T+bRb^T\ &=P_{t,t-1}+bC_tP_{t,t-1}C_t^Tb^T-bC_tP_{t,t-1}-P_{t,t-1}(bC_t)^T+bRb^T end{split}$

求跡有

$egin{split} mathrm{tr}[P_t] &=mathrm{tr}[P_{t,t-1}+bC_tP_{t,t-1}C_t^Tb^T-bC_tP_{t,t-1}-P_{t,t-1}(bC_t)^T+bRb^T]\ &=mathrm{tr}[P_{t,t-1}]+mathrm{tr}[bC_tP_{t,t-1}C_t^Tb^T]-2mathrm{tr}[bC_tP_{t,t-1}]+mathrm{tr}[bRb^T] end{split}$

令其梯度為0則有

$egin{split} frac{partial m tr[P_t]}{partial b} &=frac{partial}{partial b} mathrm{tr}[P_{t,t-1}]+frac{partial}{partial b} mathrm{tr}[bC_tP_{t,t-1}C_t^Tb^T]-2frac{partial}{partial b} mathrm{tr}[bC_tP_{t,t-1}]+frac{partial}{partial b} mathrm{tr}[bRb^T]\ &=2bC_tP_{t,t-1}C_t^T-2C_tP_{t,t-1}+2bR\ &=0\ &Rightarrow b=C_tP_{t,t-1}(C_tP_{t,t-1}C_t^T+R)^{-1} \ &Rightarrow H_t=P_{t,t-1}C_t^T[C_tP_{t,t-1}C_t^T+R]^{-1} end{split}$

則

$egin{split} hat X_t &=hat X_{t,t-1}+b(Z_t-C_that X_{t,t-1})\ &=hat X_{t,t-1}+H_t[Z_t-C_that X_{t,t-1}] end{split}$

濾波增益方程（權重）：

$H_t=P_{t,t-1}C_t^T[C_tP_{t,t-1}C_t^T+R]^{-1}$

現在我們需要求出濾波均方誤差更新矩陣（T時刻的最優均方誤差）

$egin{split} P_t &=mathbb{D}[e_t]\ &=mathbb{D}[hat X_{t,t-1}+b(Z_t-C_that X_{t,t-1})]\ &=P_{t,t-1}+bC_tP_{t,t-1}C_t^Tb^T-bC_tP_{t,t-1}-P_{t,t-1}(bC_t)^T+bRb^T\ &=P_{t,t-1}+(bC_tP_{t,t-1}C_t^T-C_tP_{t,t-1}+bR)b^T-bC_tP_{t,t-1}\ &=P_{t,t-1}-bC_tP_{t,t-1}\ &=[I-H_tC_t]P_{t,t-1} end{split}$

所以

$P_t=[I-H_tC_t]P_{t,t-1}$

注意我們多次使用到了這一結論 $mathbb{D}(X^2)=mathbb{E}(X^2)-[mathbb{E}(X)]^2$ ，當E(X)=0時 $mathbb{D}(X^2)=mathbb{E}(X^2)$

這一過程用偽代碼表示如下

也可以用這樣的簡圖來表示

以上我們是在零輸入情形下推導出的卡爾曼濾波公式，對於更一般的線性系統 $X_t=A_{t,t-1}X_{t-1}+B_{t,t-1} U_{t-1}+W_t$ ，推導的方法是十分類似的，這裡不再贅述。

在卡爾曼濾波中我們做了兩個理想化假設，第一是假設系統是線性的，第二是假設雜訊服從期望為0的正態分布。然而，許多現實世界中的系統都不是線性的，狀態和雜訊也不是高斯分布的，常用的兩種「進階」的卡爾曼濾波方法是EKF演算法和UKF演算法，這裡只做簡單介紹，詳細討論可以參見高翔的《視覺SLAM十四講》。（高翔等.視覺SLAM十四講：從理論到實踐.電子工業出版社.2017. pp.242-245）

擴展卡爾曼濾波演算法（extended Kalman filter，EKF）用高斯分布去近似非線性系統，並且在工作點附近 ${{hat x}_{k - 1}},{{ ilde x}_k}$ ，對系統進行線性近似化：

$egin{cases} fleft( {{x_{k - 1}},{v_k},{w_k}} ight) approx fleft( {{{hat x}_{k - 1}},{v_k},0} ight) + frac{{partial f}}{{partial {x_{k - 1}}}}left( {{x_{k - 1}} - {{hat x}_{k - 1}}} ight) + frac{{partial f}}{{partial {w_k}}}{w_k}\ gleft( {{x_k},{n_k}} ight) approx gleft( {{{ ilde x}_k},0} ight) + frac{{partial g}}{{partial {x_k}}}{n_k} end{cases}$

在線性系統近似下，把雜訊項和狀態都當成了高斯分布。這樣，只要估計它們的均值和協方差矩陣，就可以描述狀態，經過這樣的近似之後，EKF給出的公式和基本的卡爾曼濾波是一致的，用線性化之後的矩陣去代替卡爾曼濾波器里的轉移矩陣和觀測矩陣即可。

EKF面臨的一個重要問題是，當一個高斯分布經過非線性變換後，如何用另一個高斯分布近似它？因為，高斯分布經過一個非線性變換後，就不再是高斯分布。實際上，EKF只計算其均值與協方差，然後直接套用高斯分布近似這個非線性變換後的結果。這樣一來：

- 系統本身線性化過程中，丟掉了高階項。
- 線性化的工作點往往不是輸入狀態真實的均值，而是一個估計的均值。於是，在這個工作點下計算的F,G，也不是最好的。
- 在估計非線性輸出的均值時，EKF計算的是$muy=f(mux)$的形式。這個結果幾乎不會是輸出分布的真正期望值。協方差也是同理。

那麼，如果我們希望維持高斯分布假設，可以：

- 以EKF估計的結果為工作點，重新計算一遍EKF，直到這個工作點變化夠小。是為迭代EKF（Iterated EKF, IEKF）。
- 除了計算 $mu_y=f(mu_x)$ ，再計算其他幾個精心挑選的採樣點，然後用這幾個點估計輸出的高斯分布。是為Sigma Point KF（SPKF，或UKF）。

如果拋棄高斯分布假設，問題變為，丟掉高斯假設後，怎麼描述輸出函數的分布就成了一個問題。一種比較暴力的方式是：用足夠多的採樣點，來表達輸出的分布。這種蒙特卡羅的方式，也就是粒子濾波的思路。

如果再進一步，可以丟棄濾波器思路，不用前一個時刻的值來估計下一個時刻，而是把所有狀態看成變數，把運動方程和觀測方程看成變數間的約束，構造誤差函數，然後最小化這個誤差的二次型，這就是非線性最小二乘估計的思想，可以用Levenburg-Marquardt演算法，或者凸圖優化演算法，等等。不過，非線性優化也需要對誤差函數不斷地求梯度，並根據梯度方向迭代，因而局部線性化是不可避免的。

另一種方法，UKF（unscented Kalman filter，無跡卡爾曼濾波）主要解決一個高斯分布經過非線性變換後，怎麼用另一個高斯分布近似它。UKF的做法是找一些叫做Sigma Point的點，把這些點用g投影過去。然後，用投影之後的點做出一個高斯分布。UKF的好處就是：

- 不必線性化，也不必求導，對g沒有光滑性要求。
- 計算量隨維數增長是線性的。