Kalman Filter、Extended Kalman Filter以及Unscented Kalman Filter介紹

模型定義

如上圖所示，卡爾曼濾波(Kalman Filter)的基本模型和隱馬爾可夫模型類似，不同的是隱馬爾科夫模型考慮離散的狀態空間，而卡爾曼濾波的狀態空間以及觀測空間都是連續的，並且都屬於高斯分布，因此卡爾曼濾波又稱為linear Gaussian Markov model，它的數學定義如下：$$\underbrace{s_{t}=C s_{t-1}+G h_{t}+\gamma_{t}}_{\text { latent process }}, \quad \underbrace{x_{t}=D s_{t}+\varepsilon_{t}}_{\text { observed process }}$$其中$h_t$表示控制向量(control vector)，是已知量；$\gamma_{t} \sim N(0, Q)$表示狀態誤差，它包含了狀態轉換公式$C s_{t-1}+G h_{t}$中未考慮到的其它因素，是狀態轉換公式准確性的度量；$\varepsilon_{t} \sim N(0, V)$表示觀測誤差，是觀測精度的度量。下面舉一個簡單的例子：

• 假設有一個二維空間上的物體位置的觀測序列($x_{t} \in \mathbb{R}^{2}$)，觀測有一定誤差；該物體的狀態$s_t=[p_{t1},v_{t1},p_{t2},v_{t2}]^T$，其中$p_t$和$v_t$表示物體位置和速度，下標1和2表示方向；控制向量為$h_t=[a_{t1},a_{t2}]^T$，$a_t$表示加速度。由基本的物理公式可知$$s_{t}=\underbrace{\left[\begin{array}{cccc}{1} & {\Delta t} & {0} & {0} \\ {0} & {1} & {0} & {0} \\ {0} & {0} & {1} & {\Delta t} \\ {0} & {0} & {0} & {1}\end{array}\right]}_{C}s_{t-1}+\underbrace{\left[\begin{array}{cc} \frac{1}{2}(\Delta t)^{2} & {0} \\ {\Delta t} & {0} \\ {0} & \frac{1}{2}(\Delta t)^{2} \\ {0} & {\Delta t}\end{array}\right]}_{G}h_t+\gamma_t\text{ 以及 }x_{t}=\underbrace{\left[\begin{array}{cccc}{1} & {0} & {0} & {0} \\ {0} & {0} & {1} & {0}\end{array}\right]}_{D}s_{t}+\varepsilon_{t}$$

卡爾曼濾波的目標是已知觀測序列$x_1,x_2,\cdots,x_t$，計算當前隱藏狀態的分布函數，即$$s_{t}\left|s_{t-1} \sim N\left(C s_{t-1}+G h_{t}, Q\right), \quad x_{t}\right| s_{t} \sim N\left(D s_{t}, V\right)\quad
\Rightarrow\quad p\left(s_{t} | x_{1}, \dots, x_{t}\right);\quad 1\leq t \leq T$$注意除觀測序列以外，矩陣$C,G,Q,D,V$以及控制向量$h_t$也是給定的。 

模型求解

• 定義$S_t=(s_{t} | x_{1}, \ldots, x_{t})$，容易看出$S_t$滿足高斯分布$N(\mu_{t}, \Sigma_{t})$，$\mu_t$以及$\Sigma_t$即為需要求解的量
  • 為方便之后的計算，令$S_{t-1}=(s_{t-1} | x_{1}, \ldots, x_{t-1})=\underbrace{\mu_{t-1}}_\text{mean}+\Delta S_{t-1},\quad \Delta S_{t-1} \sim N(0,\Sigma_{t-1})$
• 定義$P_t= (s_{t} | x_{1}, \ldots, x_{t-1})$，有$P_t=CS_{t-1}+G h_{t}+\gamma_{t}$
  • 為方便之后的計算，令$P_t=\underbrace{C\mu_{t-1}+Gh_t}_{\mu_{P_t}}+\underbrace{C\Delta S_{t-1}+\gamma_t}_{\Delta P_t}$
• 定義$O_t= (x_{t} | x_{1}, \ldots, x_{t-1})$，有$O_t=DP_{t}+\varepsilon_t$
  • 為方便之后的計算，令$O_t=\underbrace{D(C\mu_{t-1}+Gh_t)}_{\mu_{O_t}}+\underbrace{DC\Delta S_{t-1}+D\gamma_t+\varepsilon_t}_{\Delta O_t}$

由上述定義可知 $$\left[\begin{array}{c} P_t \\ O_t \end{array}\right] \sim N\left(\left[\begin{array}{c} \mu_{P_t} \\ \mu_{O_t} \end{array}\right], \left[\begin{array}{cc} \Sigma_{PP} & \Sigma_{PO}\\ \Sigma_{PO}^T & \Sigma_{OO} \end{array}\right] \right)$$接下來計算協方差矩陣的這些項：

• $\Sigma_{PP}=\mathbb{E}[{\Delta P_t (\Delta P_t)^T}]=C\mathbb{E}[\Delta S_{t-1} (\Delta S_{t-1})^T]C^T+\mathbb{E}[\gamma_t\gamma_t^T]=C\Sigma_{t-1}C^T+Q$
• $\Sigma_{PO}=\mathbb{E}[{\Delta P_t (\Delta O_t)^T}]=C\mathbb{E}[\Delta S_{t-1} (\Delta S_{t-1})^T]C^TD^T+\mathbb{E}[\gamma_t\gamma_t^T]D^T=C\Sigma_{t-1}C^TD^T+QD^T$
• $\Sigma_{OO}=\mathbb{E}[{\Delta O_t (\Delta O_t)^T}]=DC\mathbb{E}[\Delta S_{t-1} (\Delta S_{t-1})^T]C^TD^T+D\mathbb{E}[\gamma_t\gamma_t^T]D^T+\mathbb{E}[\varepsilon_t\varepsilon_t^T]=DC\Sigma_{t-1}C^TD^T+DQD^T+V$

容易看出$S_t=(P_t | O_t)$，此外定義$$\hat{\mu}_t=\mu_{P_t}=C \mu_{t-1}+Gh_t，\text{  }\hat{\Sigma}_t=\Sigma_{PP}=C \Sigma_{t-1} C^{T}+Q\text{以及卡爾曼增益矩陣}K_t=\hat{\Sigma}_{t}D^T[D\hat{\Sigma}_{t}D^T+V]^{-1}$$由高斯分布的性質可知

• $\Sigma_t=\Sigma_{PP}-\Sigma_{PO}\Sigma_{OO}^{-1}\Sigma_{PO}^T=(I-K_tD)\hat{\Sigma}_t$
• $\mu_t=\mu_{P_t}+\Sigma_{PO}\Sigma_{OO}^{-1}(O_t-\mu_{O_t})=\hat{\mu}_t+K_t(x_t-D\hat{\mu}_t)$

上述求解過程可歸納為：

1. 初始化$\mu_0$以及$\Sigma_0$
2. 預測：$\hat{\mu}_t=C \mu_{t-1}+Gh_t$以及$\hat{\Sigma}_t=C \Sigma_{t-1} C^{T}+Q$
3. 計算卡爾曼增益矩陣$K_t=\hat{\Sigma}_{t}D^T[D\hat{\Sigma}_{t}D^T+V]^{-1}$
4. 更新：$\mu_t=\hat{\mu}_t+K_t(x_t-D\hat{\mu}_t)$以及$\Sigma_t=(I-K_tD)\hat{\Sigma}_t$

Extended Kalman Filter

在Extended Kalman Filter中，狀態之間的轉化以及狀態向觀測的轉化是非線性的，即$$s_t=g(s_{t-1},h_t)+\gamma_t,\text{  }x_{t}=f(s_{t})+\varepsilon_{t};\text{  其中}g,f\text{代表非線性函數}$$此時考慮使用泰勒公式將非線性函數近似為線性函數，延續上一部分的定義，有

• $P_t=g(S_{t-1},h_t)+\gamma_{t}=g(\mu_{t-1}+\delta S_{t-1},h_t)+\gamma_{t}=\underbrace{g(\mu_{t-1},h_t)}_{\mu_{P_t}(\text{i.e., }\hat{\mu}_t)}+\underbrace{J_g\Delta S_{t-1}+\gamma_{t}}_{\Delta P_t}$
• $O_t=h(P_t)+\varepsilon_t=f(\mu_{P_t}+\Delta P_t)+\varepsilon_t=f(\mu_{P_t})+J_f\Delta P_t+\varepsilon_t=\underbrace{f(\hat{\mu}_{t})}_{\mu_{O_t}}+\underbrace{J_fJ_g\Delta S_{t-1}+J_f\gamma_t+\varepsilon_t}_{\Delta O_t}$

其中$J_g$和$J_f$為Jacobian矩陣，假設狀態為$m$維向量，觀測為$n$維向量，並且$g(s,h)=[g_1(s,h),\cdots,g_m(s,h)]^T$以及$f(s)=[f_1(s),\cdots,f_n(s)]^T$，則有$$J_g=\left[\begin{array}{cccc}\frac{\partial g_1}{\partial \mu_{t-1,1}} & \frac{\partial \mu_1}{\partial s_{t-1,2}} & \cdots & \frac{\partial g_1}{\partial \mu_{t-1,m}} \\ \vdots & \vdots & \vdots & \vdots \\ \frac{\partial g_m}{\partial \mu_{t-1,1}} & \frac{\partial g_m}{\partial \mu_{t-1,2}} & \cdots & \frac{\partial g_m}{\partial \mu_{t-1,m}}\end{array}\right], \text{   }J_f=\left[\begin{array}{cccc}\frac{\partial f_1}{\partial \hat{\mu}_{t,1}} & \frac{\partial f_1}{\partial \hat{\mu}_{t,2}} & \cdots & \frac{\partial f_1}{\partial \hat{\mu}_{t,m}} \\ \vdots & \vdots & \vdots & \vdots \\ \frac{\partial f_n}{\partial \hat{\mu}_{t,1}} & \frac{\partial f_n}{\partial \hat{\mu}_{t,2}} & \cdots & \frac{\partial f_n}{\partial \hat{\mu}_{t,m}}\end{array}\right]$$容易看出Extended Kalman Filter的求解過程可歸納為：

1. 初始化$\mu_0$以及$\Sigma_0$
2. 預測：$\hat{\mu}_t=g(\mu_{t-1},h_t)$以及$\hat{\Sigma}_t=J_g \Sigma_{t-1} J_g^{T}+Q$
3. 計算卡爾曼增益矩陣$K_t=\hat{\Sigma}_{t}J_f^T[J_f\hat{\Sigma}_{t}J_f^T+V]^{-1}$
4. 更新：$\mu_t=\hat{\mu}_t+K_t[x_t-f(\hat{\mu}_t)]$以及$\Sigma_t=(I-K_tJ_f)\hat{\Sigma}_t$

Unscented Kalman Filter

Unscented Kalman Filter和Extended Kalman Filter的模型定義一樣，只是具體求解方法不同。相對於Extended Kalman Filter使用泰勒公式近似非線性函數，Unscented Kalman Filter通過選取多個樣本點(the sigma points)直接估計均值和方差。仍然延續之前的定義，假設狀態為$m$維向量，從隨機變量$S_{t-1}$中選取$2m+1$個樣本點，記為矩陣$\mathcal{X}_{t-1}$($m$行$2m+1$列)，選取方式為$$\mathcal{X}_{t-1}=\left[\begin{array}{ccc}\mu_{t-1} & \mu_{t-1}+\sqrt{(m+\lambda )\Sigma_{t-1}} & \mu_{t-1}-\sqrt{(m+\lambda )\Sigma_{t-1}} \end{array}\right]$$若將$\Sigma_{t-1}$進行Cholesky分解得到$LL^T$，則$\sqrt{\Sigma_{t-1}}=L$；或者對$\Sigma_{t-1}$進行特征值分解得到$U\Lambda U^T$(其中$\Lambda$為對角陣)，則$\sqrt{\Sigma_{t-1}}=U\Lambda^{1/2}$。接下來對每個采樣點分配權重：

• $\vec{w}_a=\left[\begin{array}{ccccc}\frac{\lambda}{m+\lambda} & \frac{1}{2(m+\lambda)} & \frac{1}{2(m+\lambda)} & \cdots & \frac{1}{2(m+\lambda)}\end{array}\right]$
• $\vec{w}_c=\left[\begin{array}{ccccc}\frac{\lambda}{m+\lambda}+(1-\alpha^2+\beta) & \frac{1}{2(m+\lambda)} & \frac{1}{2(m+\lambda)} & \cdots & \frac{1}{2(m+\lambda)}\end{array}\right]$

其中$\vec{w}_a$為求均值時的權重，$\vec{w}_c$為求協方差矩陣時的權重。針對一些參數的取值有以下建議：$$\alpha \in (0,1],\text{  }\beta=2,\text{  }\lambda=\alpha^2(m+\kappa)-m,\text{  }\kappa\geq 0$$將$\mathcal{X}_{t-1}$代入函數$g$可以得到$$\hat{\mathcal{X}}_{t}=\left[\begin{array}{cccc}g(\mathcal{X}_{t-1}^{[1]},h_t) & g(\mathcal{X}_{t-1}^{[2]},h_t)  & \cdots & g(\mathcal{X}_{t-1}^{[2m+1]},h_t)\end{array}\right]$$其中上標表示矩陣的列數，由$\hat{\mathcal{X}}_{t}$可以估計出$\hat{\mu}_t$以及$\hat{\Sigma}_t$，接下來可以通過兩種方式得到觀測的采樣點$\mathcal{Z}_t$：

1. 直接通過$\hat{\mathcal{X}}_{t}$進行計算，即$$\mathcal{Z}_{t}=\left[\begin{array}{cccc}h(\hat{\mathcal{X}}_{t}^{[1]}) & h(\hat{\mathcal{X}}_{t}^{[2]})  & \cdots & h(\hat{\mathcal{X}}_{t}^{[2m+1]}) \end{array}\right]$$
2. 通過得到的$\hat{\mu}_t$以及$\hat{\Sigma}_t$重新采樣，有公式$$\hat{\mathcal{X}}_{t}^*=\left[\begin{array}{ccc}\hat{\mu}_{t} & \hat{\mu}_{t}+\sqrt{(m+\lambda )\hat{\Sigma}_{t}} & \hat{\mu}_{t}-\sqrt{(m+\lambda )\hat{\Sigma}_{t}} \end{array}\right]$$然后計算過程同第一種方式，即$$\mathcal{Z}_{t}=\left[\begin{array}{cccc}h(\hat{\mathcal{X}}_{t}^{*[1]}) & h(\hat{\mathcal{X}}_{t}^{*[2]})  & \cdots & h(\hat{\mathcal{X}}_{t}^{*[2m+1]}) \end{array}\right]$$

最后估計觀測的均值和協方差矩陣，進而得到最終的結果，Unscented Kalman Filter的求解過程可歸納為：

1. 初始化$\mu_0$以及$\Sigma_0$
2. 預測：$\hat{\mu}_t=\sum_{j=1}^{2m+1}w_{aj}\hat{\mathcal{X}}_{t}^{[j]}$以及$\hat{\Sigma}_t=\sum_{j=1}^{2m+1}w_{cj}(\hat{\mathcal{X}}_{t}^{[j]}-\hat{\mu}_t)(\hat{\mathcal{X}}_{t}^{[j]}-\hat{\mu}_t)^T+Q$
3. 計算$\mathcal{Z}_{t}$（從上述兩種方式中選擇一種），得到$\mu_{O_t}=\sum_{j=1}^{2m+1}w_{aj}\mathcal{Z}_{t}^{[j]}$以及$\Sigma_{OO}=\sum_{j=1}^{2m+1}w_{cj}({\mathcal{Z}}_{t}^{[j]}-\mu_{O_t})({\mathcal{Z}}_{t}^{[j]}-\mu_{O_t})^T+V$
4. 計算$\Sigma_{PO}=\sum_{j=1}^{2m+1}w_{cj}(\hat{\mathcal{X}}_{t}^{[j]}-\hat{\mu}_{t})({\mathcal{Z}}_{t}^{[j]}-\mu_{O_t})^T$，注意若使用第二種方式計算$\mathcal{Z}_t$，需將公式中的$\hat{\mathcal{X}}_{t}$替換為$\hat{\mathcal{X}}_{t}^*$
5. 計算卡爾曼增益矩陣$K_t=\Sigma_{PO}\Sigma_{OO}^{-1}$
6. 更新：$\mu_t=\hat{\mu}_t+K_t[x_t-\mu_{O_t}]$以及$\Sigma_t=\hat{\Sigma}_t-\Sigma_{PO}\Sigma_{OO}^{-1}\Sigma_{PO}^T=\hat{\Sigma}_t-K_t\Sigma_{OO}K_t^T$