卡爾曼濾波

卡爾曼濾波

預測方程

$\hat x_k ' = A \hat x_{k-1} + B \mu_k \tag{1}$
${P_k}' = AP_{k-1}A^T + Q \tag{2}$

更新方程

$K_k = {P_k}' H^T \left(H {P_k}'H^T + R \right)^{-1} \tag{3}$
$\hat x_k = {\hat x_k}' + K_k \left(y_k - H {\hat x_k}'\right) \tag{4}$
$P_k = \left(I - K_k H\right){P_k}' \tag{5}$
${\hat x_k}'$ : 爲預測值
$\hat x_{k - 1}$ : 爲估計值
$H$ : 真實狀態到觀測狀態的變換
$K_k$ : 卡爾曼增益
$Q$ : 預測噪聲的協方差
$R$ : 觀測噪聲的協方差
${P_k}'$ : 預測的協方差
$P_k$ : 估計的協方差
$y_k$ : 觀測值
$A$ : 狀態轉移矩陣
$B$ : 控制矩陣
$x_k$ : 真實值（我們無法得到）

預測方程 (1)

  假設，一個人開着一輛車在一條比值的道路上行駛，我們的目的是要知道這個人的狀態信息 ：
$x_k = \begin{bmatrix} p_k \\ v_k \end{bmatrix} \tag{6}$ 並假設車的加速度爲 $\mu_k$。
$p_k = p_{k - 1} + v_{k - 1} \Delta t + \frac{1}{2} \mu_k {\Delta t}^2 \tag{7}$
$v_k = v_{k - 1} + \mu_k \Delta t \tag{8}$
根據 $(6),(7),(8)$ 公式得到：
$x_k = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \begin{bmatrix} p_{k - 1} \\ v_{k - 1} \end{bmatrix} + \begin{bmatrix} \frac{1}{2}{\Delta t}^2 \\ \Delta t \end{bmatrix} \mu_k \tag{9}$
令 :
$A = \begin{bmatrix} 1 & \Delta t \\ 0 & 1\end{bmatrix}$
$B = \begin{bmatrix} \frac{1}{2} {\Delta t}^2 \\ \Delta t \end{bmatrix}$
我們稱 $A,B$分別爲 狀態轉移矩陣和控制矩陣。$\hat x_{k - 1}$ 爲上一次卡爾曼的估計。
這樣我們就得到了一個線性預測（後驗）方程 ： $\hat x_k ' = A \hat x_{k-1} + B \mu_k$

更新方程(3)

  根據預測方程 (1)*** 得到的方程形式我們可以系統狀態預測方程*:
$x_k = A x_{k - 1} + B \mu_k + \omega_k \tag{7}$
其中 $\omega_k$ 爲均值爲 $0$，協方差矩陣爲 $Q$，且服從正態分佈的預測噪聲 （預測模型本身帶有的噪聲）。
   小車的真實狀態 $x_k$ 我們其實是無法得知的，我們只能通過觀測值來對真實值進行估計。現在我們在路上安裝一個設備來測定小汽車的位置，觀測到的值記爲 $y_k$ ，這樣從小汽車的真實狀態到其觀測狀態會存在一個變換關係，我們記爲 $H$ ，並且該 $H$ 也是一個線性函數，此時我們定義一個觀測方程：
$y_k = H x_k + v_k$
其中 $v_k$ 均值爲 $0$ ，協方差矩陣爲 $R$，且服從正態分佈的的測量噪聲（測量過程中認爲存在噪聲）。
  現在有兩個值，一個是傳感器的測量值，一個是前一狀態得到的預測值，並且它們都是服從高斯分佈的。如果我們想知道這兩種情況都可能發生的概率，將這兩個高斯分佈相乘就可以了。
  用一維的高斯分佈來分析，具有方差 $\sigma ^2$ 和 $\mu$ 的高斯曲線可以用下式表示：
$N\left(x, \mu, \sigma\right) = \frac{1}{\sigma \sqrt{2 \pi}} e ^{-\frac{\left(x - \mu\right)^2}{2 \sigma^2}}$
如果把兩個高斯曲線相乘 $N\left(x, \mu_0, \sigma_0\right) \cdot N\left(x, \mu_1, \sigma_1\right) = N\left(x, \mu', \sigma'\right)$

$N\left(x, \mu_0, \sigma_0\right) \cdot N\left(x, \mu_1, \sigma_1\right)$
$=\frac{1}{\sigma_0 \sqrt{2 \pi}} e^{-\frac{\left(x - \mu_0\right)^2}{2 {\sigma_0}^2}} \cdot \frac{1}{\sigma_1\sqrt{2 \pi}} e^{-\frac{\left(x - \mu_1\right)^2}{2 {\sigma_1}^2}}$
$=\frac{1}{\sqrt{2 \pi}} \cdot \frac{1}{\sigma_0 \sigma_1 \sqrt{2 \pi}} e^{-\frac{{\sigma_1}^2 \left(x - \mu_0\right)^2 + \sigma_0^2\left(x - \mu_1\right)^2}{2\left(\sigma_0 \sigma_1\right)^2}}$
$=\frac{\frac{1}{\sqrt{\sigma_0^2 + \sigma_1^2}}}{\sqrt{2 \pi} \cdot \frac{1}{\sqrt{\sigma_0^2 + \sigma_1^2}}} \cdot \frac{1}{\sigma_0 \sigma_1 \sqrt{2 \pi}} e^{-\frac{\left(x - \frac{\mu_0 \sigma_1^2 + \mu_1 \sigma_0^2}{\sigma_0^2 + \sigma_1^2}\right)^2}{2\left(\frac{\sigma_0 \sigma_1}{\sqrt{\sigma_0^2 + \sigma_1^2}}\right)^2}}$

其中
$\mu' = \frac{\mu_0 \sigma_1^2 + \mu_1\sigma_0^2}{\sigma_0^2 + \sigma_1^2} = \mu_0 + \frac{\left(\mu_1 - \mu_0\right) \sigma_0^2}{\sigma_0^2 + \sigma_1^2} \tag{8}$
${\sigma'}^2 = \frac{\sigma_0^2 \sigma_1^2}{\sigma_0^2 + \sigma_1^2} = \sigma_0^2 - \frac{\sigma_0^4}{\sigma_0^2 + \sigma_1^2} \tag{9}$
令$k = \frac{\sigma_0^2}{\sigma_0^2 + \sigma_1^2}$
$\mu' = \mu_0 + k\left(\mu_1 - \mu_0 \right) \tag{10}$
${\sigma'}^2 = \sigma_0^2 - k \sigma_0^2 \tag{11}$
$(10)，(11)$式寫爲矩陣模式， $\Sigma$ 表示高斯分佈的協方差，$\vec \mu$ 表示每個維度的均值則：
$K = \Sigma_0\left(\Sigma_0 + \Sigma_1\right)^{-1} \tag{12}$
$\vec{\mu'} = \vec{\mu_0} + K\left(\vec{\mu_1} - \vec{\mu_0}\right)\tag{13}$
$\Sigma' = \Sigma_0 - K\Sigma_0\tag{14}$

預測方程 (2)

  真實值 $x_k$ 與預測值 ${\hat x_k}'$ 之間的誤差：
${e_k}' = x_k - {\hat x_k}'$
${P_k}' = E\left[\left(x_k - {\hat x_k}'\right)\left(x_k - {\hat x_k}'\right)^T\right]$
$\hat x_k' = A \hat x_{k - 1} + B \mu_k$
同樣的方程也適用於真實值
$x_k = A x_{k - 1} + B \mu_k + \omega_k$
根據以上方程組得到：
${P_k}' = E\left[\left(A e_{k - 1}\right)\left(A e_{k - 1}\right)^T\right] + E\left[\omega_k \omega_k^T\right] = AP_{k - 1}A^T + Q$

更新方程（4）

$\hat x_k = {\hat x_k}' + K_k\left(y_k - H{\hat x_k}'\right)$
卡爾曼估計在預測的結果基礎上再加上一個小的調整，用最小二乘來看待此問題，其中 ${\hat x_k}'$相當於初始值， 其中 $y_k - H {\hat x_k}'$相當於殘差，$K_k$ 相當於雅各比矩陣。

卡爾曼增益（3）

   真實值 $x_k$ 與估計值 $\hat x_k$ 之間的誤差：
$e_k = x_k - \hat x_k$
$\hat x_k = {\hat x_k}' + K_k\left(y_k - H{\hat x_k}'\right)$
$y_k = H x_k + v_k$
根據以上方程得到：
$e_k = \left(I - K_k H\right)\left(x_k - {\hat x_k}'\right) - K_kv_k$
$P_k = E\left[e_ke_k^T\right] = \left(I-K_kH\right){P_k}'\left(I - K_kH\right)^T + K_kRK_k^T \tag{15}$
卡爾曼濾波本質是最小均方差估計，而均方差是$P_k$ 的跡，將上式展開求跡：
$tr\left(P_k\right) = tr\left({P_k}'\right) - s tr\left(K_kH{P_k}'\right) + tr\left(K_k\left(H{P_k}'H^T + R\right)K_k^T\right)$
$\frac{\delta P_k}{\delta K_k} = -2\left(H{R_k}'\right)^T + 2 K_k \left(H{P_k}'H^T + R\right)$
令上式等於 $0$ :
$K_k = {P_k}'H^T\left(H{P_k}'H^T + R\right)^{-1}$

更新方程（5）

展開$(15)$式得到：
$P_k = {P_k}' - K_kH{P_k}' - {P_k}'H^TK_k^T + {P_k}'H^T\left(H{P_k}'H^T + R\right)^{-1}\left(H{P_k}'H^T + R\right)K_k^T$
將 $K_k$代入上式右邊最後一項，$K_k^T$ 保持原樣
$P_k = \left(I - K_kH\right){P_k}'$