卡爾曼濾波(Kalman Filter)介紹

Kalman Filter幫助我們在動態系統中即使是不完全及包含雜訊的測量也能有很好的估計。

Kalman Filter應用實例：

試想現在我們要監控火箭裡燃燒室的內部溫度，我們當然不能直接將感測溫度的傳感器放置在裡面，因為內部的超高溫會使傳感器融化，因此只能將感測器安裝在較涼的外側表面，那我們要如何以外部的溫度更好的估測內部溫度，這裡就可以使用Kalman Filter來幫助我們做估測。

第二個例子則是GPS定位系統，GPS訊號可能在車子進入隧道後訊號變弱、雜訊增加，車子的系統要依賴自己內部儀器(IMU、里程表)輔助計算目前所在位置，Kalman Filter能幫助我們融合各種測量值找到汽車精確位置的最佳估計值。

簡單的說，Kalman Filter能融合各種測量值得到最佳估計值。(Wiki是這樣寫：卡爾曼濾波會根據各測量量在不同時間下的值，考慮各時間下的聯合分布，再產生對未知變數的估計，因此會比只以單一測量量為基礎的估計方式要準。)

Kalman Filter Model：

首先，推薦看這個blog，講解得非常清楚：How a Kalman filter works, in pictures
 

參數意義：
    x : filter state estimate
    P : covariance matrix
    Q : process uncertainty/noise
    R : measurement uncertainty/noise
    H : measurement function
    F : state transition matrix
    u : control vector
    B : control transition matrix
    K : kalman gain

從上圖的流程中我們可以看到 Kalman Filter分成兩步驟：首先是 predict，再來是 update。

  Prediction Step：

$$\hat{x}_{k} = F_{k}\hat{x}_{k-1}+B_{k}\vec{u}_{k}$$
$$P_{k} = F_{k}P_{k-1}F_{k}^{T}+Q$$

這裡比較像我們一般在建立馬可夫鏈模型一樣，這裡我們以等速運行的車為例，車子的狀態有四個變數：x座標、y座標、x方向速度、y方向速度，車子知道的只有速度資訊。
$$x=\left [\begin{array}{cc}x & y & v_x & v_y \end{array}\right ] ^{T}$$
$$F=\left [\begin{array}{cc}1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right ]$$
$$x F = \left [\begin{array}{cc}x+v_x\Delta t & y+v_y\Delta t  & v_x & v_y \end{array}\right ]^{T}$$
$B$和$u$是人為操縱給的影響，像是我們操縱遙控器讓車子進行加速，但若沒有這樣的人為操控的話，就不須計算$B$和$u$。而$P$和$Q$的計算是考量進Uncertainty。
$$P_0=\left [\begin{array}{cc}\sigma_{x}^2 & 0 & 0 & 0 \\ 0 & \sigma_{y}^2 & 0 & 0 \\ 0 & 0 & \sigma_{v_x}^2 & 0 \\ 0 & 0 & 0 & \sigma_{v_y}^2 \end{array}\right ]$$
$$Q=\left [\begin{array}{cc}\sigma_{x}^2 & \sigma_{xy} & \sigma_{xx_v} & \sigma_{xy_v} \\ \sigma_{yx} & \sigma_{y}^2 & \sigma_{yx_v} & \sigma_{yy_v} \\ \sigma_{x_vx} & \sigma_{x_vy} & \sigma_{x_v}^2 & \sigma_{x_vy_v} \\ \sigma_{y_vx} & \sigma_{y_vy} & \sigma_{y_vx_v} & \sigma_{y_v}^2 \end{array}\right ]$$

  Update Step：

$$K_{k} = P_{k}H^T(HP_kH^T+R)^{-1}$$
$$\hat{x}'_{k} = \hat{x}_k + K_k(\vec{z}_k-H\hat{x}_k)$$
$$P_{k}' = P_k - K_kHP_k$$
update步驟相當於修正predict步驟得出的state值。
$H$是measurement matrix，在我們上面的等速模型實際測量的值只有$v_x$和$v_y$兩個速度值，因此
$$H=\left [\begin{array}{cc}0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1\end{array}\right ]$$
$$R=\left [\begin{array}{cc}\sigma_{v_x}^2 & 0 \\ 0 & \sigma_{v_y}^2 \end{array}\right ]$$
而$\vec{z}_k$是我們這次的k時間點測到的速度值$\left [\begin{array}{cc}v_x' & v_y' \end{array}\right ]^{T}$
經過update後的$\hat{x}'_{k}$即我們經過修正過後的狀態，即是Kalman Filter得到的目前時間點的最佳估測值！


上面等速模型的例子是源於這裡，這個也推薦可以看一下：Kalman-Filter-CV.ipynb

寫Python若想要用Kalman Filter，參考這個：FilterPy

參考資料：

  1. Matlab youtube: https://www.youtube.com/watch?v=mwn8xhgNpFY
  2. https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/
  3. https://github.com/balzer82/Kalman
  4. https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python     (這裡寫的很細，想更了解細節請看這裡)