# Kalman Filter 是如何運作的
###### tags: `Kalman filter` `Interest` `Tracking` `uncertainty`
資料來源:[How a Kalman filter works, in pictures](http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/)

## 什麼是卡爾曼濾波器?
當遇到任何在動態環境有不確定訊號的時候皆可使用，他將以有邏輯的方法去猜測真實的狀態值。
## Kalman filter 是如何看待問題的
當我們要追蹤的是位置和速度的狀態時，我們假設其變數皆為高斯機率分布。
$\vec{x} = \begin{bmatrix}
p\\
v
\end{bmatrix}$
當位置和速度為無相關性的高斯機率分布:
當位置和速度有正相關性時:
## 將問題轉換成數學模型
$\mathbf{\hat{x}_k}$為最有可能的狀態(高斯機率的平均數)，$\mathbf{P}_k$則為便異數矩陣。

\begin{equation}
\begin{aligned}
\mathbf{\hat{x}}_k &= \begin{bmatrix}
\text{position}\\
\text{velocity}
\end{bmatrix}\\
\mathbf{P}_k &=
\begin{bmatrix}
\Sigma_{pp} & \Sigma_{pv} \\
\Sigma_{vp} & \Sigma_{vv} \\
\end{bmatrix}
\end{aligned}
\end{equation}

再者，我們需要從現在的狀態($x_{k-1}$)預測未來(時間點$x_k$)的值，因此我們定義預測的矩陣$\mathbf{F_k}$。
從直觀的角度出發，我們會用基礎的物體運動學去預測物體的移動。
而基本的卡氏運動模型如下:

\begin{split}
\color{deeppink}{p_k} &= \color{royalblue}{p_{k-1}} + \Delta t &\color{royalblue}{v_{k-1}} \\
\color{deeppink}{v_k} &= &\color{royalblue}{v_{k-1}}
\end{split}
從上述方程式可推得:

\begin{align}
\color{deeppink}{\mathbf{\hat{x}}_k} &= \begin{bmatrix}
1 & \Delta t \\
0 & 1
\end{bmatrix} \color{royalblue}{\mathbf{\hat{x}}_{k-1}} \\
&= \mathbf{F}_k \color{royalblue}{\mathbf{\hat{x}}_{k-1}}
\end{align}

我們便得到了預測矩陣 $\mathbf{F}_k$
若我們每個狀態的值乘上矩陣$\mathbf{A}$，那我們的變異數矩陣會產生以下的變化：
\begin{equation}
\begin{split}
Cov(x) &= \Sigma\\
Cov(\color{firebrick}{\mathbf{A}}x) &= \color{firebrick}{\mathbf{A}} \Sigma \color{firebrick}{\mathbf{A}}^T
\end{split}
\end{equation}

> 證明$Cov(\mathbf{A}x)=\mathbf{A} \Sigma \mathbf{A}^T$
> 

結合上面推導的式子可得:
\begin{equation}
\begin{split}
\color{deeppink}{\mathbf{\hat{x}}_k} &= \mathbf{F}_k \color{royalblue}{\mathbf{\hat{x}}_{k-1}} \\
\color{deeppink}{\mathbf{P}_k} &= \mathbf{F_k} \color{royalblue}{\mathbf{P}_{k-1}} \mathbf{F}_k^T
\end{split}
\end{equation}

## 外界施力的影響
雖然我們推導出了下個時間點的預測模型，但我們尚未考慮到其他變化是跟物體狀態無關的外界力量，例如，當我今天需要煞停自走車，那我必要對其施力(可能是剎車器或馬達)。我們假設施力為$\color{darkorange}{\vec{\mathbf{u}_k}}$，此施力所對應到的加速度$\color{darkorange}{a}$對於我們的卡式座標狀態的影響如下:
\begin{split}
\color{deeppink}{p_k} &= \color{royalblue}{p_{k-1}} + {\Delta t} &\color{royalblue}{v_{k-1}} + &\frac{1}{2} \color{darkorange}{a} {\Delta t}^2 \\
\color{deeppink}{v_k} &= &\color{royalblue}{v_{k-1}} + & \color{darkorange}{a} {\Delta t}
\end{split}
轉換成矩陣型式:
\begin{equation}
\begin{split}
\color{deeppink}{\mathbf{\hat{x}}_k} &= \mathbf{F}_k \color{royalblue}{\mathbf{\hat{x}}_{k-1}} + \begin{bmatrix}
\frac{\Delta t^2}{2} \\
\Delta t
\end{bmatrix} \color{darkorange}{a} \\
&= \mathbf{F}_k \color{royalblue}{\mathbf{\hat{x}}_{k-1}} + \mathbf{B}_k \color{darkorange}{\vec{\mathbf{u}_k}}
\end{split}
\end{equation}

## 外界的不確定性(**uncertainty**)
狀態是可以被預期的，當它只受到可預期的變化(例如本身的狀態變化或是已知的外界施力)。但如果有我們所不知道的力在影響我們的系統時，我們要如何預測物體的狀態呢?

* 將不確定性加入模型
我們會在每個預測的步驟加入不確定性，並以此來描述不確定性的模型，如下圖所示
* 定義不確定性
因為高斯分布是多麼的厲害，我們將會假設每個點$\mathbf{\hat{x}}_{k-1}$會依據變異數$\mathbf{Q}_k$的高斯分布隨機移動。
這些點這些點所形成的區域，這些區域的變異數雖然不同，但平均值是相同的。

* 加入不確定性的矩陣型式
我們將原本的變異數矩陣擴展(加入$\mathbf{Q}_k$)，得到以下的方程式
\begin{equation}
\begin{split}
\color{deeppink}{\mathbf{\hat{x}}_k} &= \mathbf{F}_k \color{royalblue}{\mathbf{\hat{x}}_{k-1}} + \mathbf{B}_k \color{darkorange}{\vec{\mathbf{u}_k}} \\
\color{deeppink}{\mathbf{P}_k} &= \mathbf{F_k} \color{royalblue}{\mathbf{P}_{k-1}} \mathbf{F}_k^T + \color{mediumaquamarine}{\mathbf{Q}_k}
\end{split}
\end{equation}

最後會得到以下的結論(*文字顏色與前面方程式有所呼應*):
**<font color="deeppink">最有可能的狀態</font>為<font color ="royalblue">上一次狀態</font>加上<font color="darkorange">已知外力的修正項</font>**
**<font color="deeppink">新的變異矩陣(uncertainty)</font>為<font color ="royalblue">舊的變異矩陣</font>的預測項加上<font color="mediumaquamarine">外界的變異數矩陣</font>**

## 從測量值估算出狀態

當我們估計狀態，會需要一個或多的感測器。
而我們感測到的值和我們需要的狀態通常需要一個轉換矩陣$\mathbf{H}_k$。
例如一般IMU只提供加送度的感測值，但我們會需要估算位置和速度的值。
如下圖所示:
因此我們可以定義感測的模型:
\begin{equation}
\begin{aligned}
\vec{\mu}_{\text{expected}} &= \mathbf{H}_k \color{deeppink}{\mathbf{\hat{x}}_k} \\
\mathbf{\Sigma}_{\text{expected}} &= \mathbf{H}_k \color{deeppink}{\mathbf{P}_k} \mathbf{H}_k^T
\end{aligned}
\end{equation}

但是這個世界就是充滿著雜訊，偵測器之間的轉換也是。好險Kalman filter 能處理好雜訊的濾波。
因此當我們感測器的轉換矩陣轉換狀態(我們前面所預測"應該是準確"的狀態值)所得到的值和我們觀測到的值不一樣時(如下圖所示)

我們會定義這些不確定性的變異數為$\color{mediumaquamarine}{\mathbf{R}_k}$，觀測分布的平均值為$\color{yellowgreen}{\vec{\mathbf{z}_k}}$。
這樣我們會擁有兩個高斯分布，一個是**事前觀測後所預測之狀態**的高斯分布(<font color ="pink">粉紅色</font>)，另一個則是**感測值**的高斯分布(<font color= "green">綠色</font>)。如下圖所示:

交集兩個高斯分布的值則為現在最有可能的狀態值分布。
 => 

## 高斯分布的集合

變異數為$\sigma^2$平均為$\mu$的一維高斯分布定義如下:
\begin{equation}
\mathcal{N}(x, \mu,\sigma) = \frac{1}{ \sigma \sqrt{ 2\pi } } e^{ -\frac{ (x – \mu)^2 }{ 2\sigma^2 } }
\end{equation}
而當我們要交集兩個高斯分布時(如下圖所示)，

我們需要知道如何轉換為交集的高斯分，我們需要將兩個高斯分布相乘。
\begin{equation}
\mathcal{N}(x, \color{fuchsia}{\mu_0}, \color{deeppink}{\sigma_0}) \cdot \mathcal{N}(x, \color{yellowgreen}{\mu_1}, \color{mediumaquamarine}{\sigma_1}) \stackrel{?}{=} \mathcal{N}(x, \color{royalblue}{\mu’}, \color{mediumblue}{\sigma’})
\end{equation}

經過證明
> from[證明出處](http://www.ams.sunysb.edu/~zhu/ams570/Bayesian_Normal.pdf)
> 預測的機率為事前機率和感測值的結合，應用到貝式定理(bayesian theorem)
> 得證

我們可以知道
\begin{equation}
\begin{aligned}
\color{royalblue}{\mu’} &= \mu_0 + \frac{\sigma_0^2 (\mu_1 – \mu_0)} {\sigma_0^2 + \sigma_1^2}\\
\color{mediumblue}{\sigma’}^2 &= \sigma_0^2 – \frac{\sigma_0^4} {\sigma_0^2 + \sigma_1^2}
\end{aligned}
\end{equation}

我們定義\begin{equation}
\color{purple}{\mathbf{k}} = \frac{\sigma_0^2}{\sigma_0^2 + \sigma_1^2}
\end{equation}
則可簡化上式為
\begin{equation}
\begin{split}
\color{royalblue}{\mu’} &= \mu_0 + &\color{purple}{\mathbf{k}} (\mu_1 – \mu_0)\\
\color{mediumblue}{\sigma’}^2 &= \sigma_0^2 – &\color{purple}{\mathbf{k}} \sigma_0^2
\end{split}
\end{equation}
我們將上面一維的證明擴展到二維：若$\Sigma$為高斯分布的變異數矩陣，則可推得\begin{equation}
\color{purple}{\mathbf{K}} = \Sigma_0 (\Sigma_0 + \Sigma_1)^{-1}
\end{equation}
且
\begin{equation}
\begin{split}
\color{royalblue}{\vec{\mu}’} &= \vec{\mu_0} + &\color{purple}{\mathbf{K}} (\vec{\mu_1} – \vec{\mu_0})\\
\color{mediumblue}{\Sigma’} &= \Sigma_0 – &\color{purple}{\mathbf{K}} \Sigma_0
\end{split}
\end{equation}

<font color ="purple">$\mathbf{K}$</font>就是(大名鼎鼎的)卡爾曼權重值(Kalman gain)。

## 將所有的推論結合
第一，我們擁有兩個分布，預測的分布$(\color{fuchsia}{\mu_0}, \color{deeppink}{\Sigma_0}) = (\color{fuchsia}{\mathbf{H}_k \mathbf{\hat{x}}_k}, \color{deeppink}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T})$以及觀測的分布$(\color{yellowgreen}{\mu_1}, \color{mediumaquamarine}{\Sigma_1}) = (\color{yellowgreen}{\vec{\mathbf{z}_k}}, \color{mediumaquamarine}{\mathbf{R}_k})$，從上面的推導我們可以得知
\begin{equation}
\begin{aligned}
\mathbf{H}_k \color{royalblue}{\mathbf{\hat{x}}_k’} &= \color{fuchsia}{\mathbf{H}_k \mathbf{\hat{x}}_k} & + & \color{purple}{\mathbf{K}} ( \color{yellowgreen}{\vec{\mathbf{z}_k}} – \color{fuchsia}{\mathbf{H}_k \mathbf{\hat{x}}_k} ) \\
\mathbf{H}_k \color{royalblue}{\mathbf{P}_k’} \mathbf{H}_k^T &= \color{deeppink}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} & – & \color{purple}{\mathbf{K}} \color{deeppink}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T}
\end{aligned}
\end{equation}其中
\begin{equation}
\color{purple}{\mathbf{K}} = \color{deeppink}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} ( \color{deeppink}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} + \color{mediumaquamarine}{\mathbf{R}_k})^{-1}
\end{equation}

簡化過後，我們可以得到最有可能的預估值 $\color{royalblue}{\mathbf{\hat{x}}_k’}$和其變異數 $\color{royalblue}{\mathbf{P}_k’}$:
\begin{equation}
\begin{split}
\color{royalblue}{\mathbf{\hat{x}}_k’} &= \color{fuchsia}{\mathbf{\hat{x}}_k} & + & \color{purple}{\mathbf{K}’} ( \color{yellowgreen}{\vec{\mathbf{z}_k}} – \color{fuchsia}{\mathbf{H}_k \mathbf{\hat{x}}_k} ) \\
\color{royalblue}{\mathbf{P}_k’} &= \color{deeppink}{\mathbf{P}_k} & – & \color{purple}{\mathbf{K}’} \color{deeppink}{\mathbf{H}_k \mathbf{P}_k}
\end{split}
\end{equation}

其中的 $\color{purple}{\mathbf{K}} = \color{deeppink}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} ( \color{deeppink}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} + \color{mediumaquamarine}{\mathbf{R}_k})^{-1}$。
這邊Kalman filter 就差不多推導完了。

但...人生就是這個**BUT**
上述的推導都是基於線性系統，一但我們遇到非線性系統，我們將會採用
extended Kalman filter(怎麼開了新坑阿= =，未來有<font color=red>機會</font>再寫吧XD~)