（二）. 細說Kalman濾波：The Kalman Filter

本文為原創文章，轉載請注明出處，http://www.cnblogs.com/ycwang16/p/5999034.html

前面介紹了Bayes濾波方法，我們接下來詳細說說Kalman濾波器。雖然Kalman濾波器已經被廣泛使用，也有很多的教程，但我們在Bayes濾波器的框架上，來深入理解Kalman濾波器的設計，對理解采用Gaussian模型來近似狀態分布的多高斯濾波器(Guassian Multi-Hyperthesis-Filter)等都有幫助。

一. 背景知識回顧

1.1 Bayes濾波

首先回顧一下Bayes濾波. Bayes濾波分為兩步：1.狀態預測；和 2.狀態更新

1. 狀態預測，基於狀態轉移模型：

$\overline {bel} ({x_t}) = \int {p({x_t}|{u_t},{x_{t - 1}})} \;bel({x_{t - 1}})\;d{x_{t - 1}}$

2. 狀態更新，基於新的觀測

$bel({x_t}) = \;\eta \,p({z_t}|{x_t})\,\overline {bel} ({x_t})$

我們可以看到，我們的目的是計算$x_t$的后驗概率，如果$bel({x_t})$是任意分布，我們需要在$x_t$的所有可能取值點上，計算該取值的概率，這在計算上是難於實現的。這一計算問題可以有多種方法來近似，比如利用采樣的方法，就是后面要講的粒子濾波和無跡Kalman濾波。

這節要說的近似方法是，當假設$bel({x_t})$服從Gauss分布，那么我們只需要分布的均值和方差就可以完全描述$bel({x_t})$，而無需在$x_t$的每個可能取值點上進行概率計算。這也是用高斯分布來近似$bel({x_t})$的好處，因為我們在每一個時刻，只需要計算均值$\mu_t$和方差$\Sigma_t$這兩個數值，就可以對$bel({x_t})$完全描述，所以我們就可以推導出這兩個數值的遞推公式，從而在每個時刻由這兩個數值的遞推公式完全獲得狀態估計，這就是The Kalman Filter的基本思想。

1.2 正態分布(Guassian Distribution)

然后我們再回顧一下正態分布的基礎知識。正態分布是一種特殊的概率分布，分布的形態完全由二階矩決定。一元高斯分布表述如下：

$$\begin{array}{l}
p(x)\sim N(\mu ,{\sigma ^2}):\\
p(x) = \frac{1}{{\sqrt {2\pi } \sigma }}{e^{ - \frac{1}{2}\frac{{{{(x - \mu )}^2}}}{{{\sigma ^2}}}}}
\end{array}$$

其中，一階矩為均值$\mu$表示期望值，二階矩為方差$\sigma$表示分布的不確定程度。

多元高斯分布的表達式為：

$$\begin{array}{l}
p({\bf{x}})\sim {\rm N}({\bf{\mu }},{\bf{\Sigma }}):\\
p({\bf{x}}) = \frac{1}{{{{(2\pi )}^{d/2}}{{\left| {\bf{\Sigma }} \right|}^{1/2}}}}{e^{ - \frac{1}{2}{{({\bf{x}} - {\bf{\mu }})}^t}{{\bf{\Sigma }}^{ - 1}}({\bf{x}} - {\bf{\mu }})}}
\end{array}$$

同樣，一階矩為$\bf{\mu}$表示各元變量的期望值，二階矩為方差矩陣$\bf{\Sigma}$表示各元變量的不確定程度。

1.3 正態分布的特點

在線性變換下，一旦高斯，代代高斯。

首先，高斯變量線性變換后，仍為高斯分布，均值和方差如下：

$\left. {\begin{array}{*{20}{l}}
{X\sim N(\mu ,\Sigma )}\\
{Y = AX + B\quad \;}
\end{array}} \right\}\quad  \Rightarrow \quad Y\sim N(A\mu  + B,A\Sigma {A^T})$

然后，兩個高斯變量線性組合，仍為高斯分布，均值和方差如下：

$\left. {\begin{array}{*{20}{c}}
{{X_1}\sim N({\mu _1},\sigma _1^2)}\\
{{X_2}\sim N({\mu _2},\sigma _2^2)}
\end{array}} \right\} \Rightarrow p({X_1} + {X_2})\sim N\left( {{\mu _1} + {\mu _2},\sigma _1^2 + \sigma _2^2 + 2\rho {\sigma _1}{\sigma _2}} \right)$

最后，兩個相互獨立的高斯變量的乘積，仍然為高斯分布，均值和方差如下：

$\left. {\begin{array}{*{20}{c}}
{{X_1}\sim N({\mu _1},\sigma _1^2)}\\
{{X_2}\sim N({\mu _2},\sigma _2^2)}
\end{array}} \right\} \Rightarrow p({X_1}) \cdot p({X_2})\sim N\left( {\frac{{\sigma _2^2}}{{\sigma _1^2 + \sigma _2^2}}{\mu _1} + \frac{{\sigma _1^2}}{{\sigma _1^2 + \sigma _2^2}}{\mu _2},\quad \frac{{\sigma _1^2\sigma _2^2}}{{\sigma _1^2 + \sigma _2^2}}} \right)$

正因為高斯分布有這些特點，所以，在Bayes濾波公式中的隨機變量的加法、乘法，可以用解析的公式計算均值和方差，這使得Bayes濾波的整個計算過程非常簡便，即Kalman濾波器的迭代過程。

二. Kalman濾波

2.1 Kalman濾波的模型假設

Kalman濾波所解決的問題，是對一個動態變化的系統的狀態跟蹤的問題，基本的模型假設包括：1）系統的狀態方程是線性的；2）觀測方程是線性的；3）過程噪聲符合零均值高斯分布；4）觀測噪聲符合零均值高斯分布；從而，一直在線性變化的空間中操作高斯分布，狀態的概率密度符合高斯分布。

1. 狀態方程
   ${x_t} = {A_t}{x_{t - 1}} + {B_t}{u_t} + {\varepsilon _t}$
2. 觀測方程
   ${z_t} = {H_t}{x_t} + {\delta _t}$

其中過程噪聲${\varepsilon _t}$假設符合零均值高斯分布；觀測噪聲${\delta _t}$假設符合零均值高斯分布。對於上述模型，我們可以用如下參數描述整個問題：

2.2 Kalman濾波器的模型

1. $x_t$，$n$維向量，表示$t$時刻觀測狀態的均值。
2. $P_t$，$n*n$方差矩陣，表示$t$時刻被觀測的$n$個狀態的方差。
3. $u_t$，$l$維向量，表示$t$時刻的輸入
4. $z_t$，$m$維向量，表示$t$時刻的觀測
5. ${A_t}$，$n*n$矩陣，表示狀態從$t-1$到$t$在沒有輸入影響時轉移方式
6. ${B_t}$，$n*n$矩陣，表示$u_t$如何影響$x_t$
7. ${H_t}$，$m*n$矩陣，表示狀態$x_t$如何被轉換為觀測$z_t$
8. ${R_t}$，$n*n$矩陣，表示過程噪聲${\varepsilon _t}$的方差矩陣
9. $Q_t$，$m*m$矩陣，表示觀測噪聲${\delta _t}$的方差矩陣

圖1. 在沒有觀測情況下，系統狀態的從$t-1$到$t$的轉移方式

圖1給出了在沒有觀測，僅有輸入$u_t$時，狀態變量的均值和方差從$t-1$到$t$的轉移方式，可見均值和方差的計算，完全是基於高斯分布的線性變化的方法來算的。

圖2. Kalman 濾波解決在收到t時刻的輸入$u_t$和觀測$z_t$的情況下，更新狀態$x_t$的問題

圖2給出了Kalman濾波所解決的問題，即在獲得t時刻的輸入和觀測的情況下，如何更新$x_t$的均值和方差的問題。當然$u_t$和$z_t$也並不是每一個時刻都需要同時獲得，就像貝葉斯濾波一樣，可以在獲得$u_t$時就做一次狀態預測，在獲得$z_t$時做一次狀態更新。

2.3 Kalman濾波算法

Kalman濾波整體算法如下：

Kalman Filter ($x_{t-1}, P_{t-1}, u_t, z_t$)

• Prediction
• ${\overline x _t} = {A_t}{x_{t - 1}} + {B_t}{u_t}$
• ${\overline P _t} = {A_t}{P_{t - 1}}A_t^T + {R_t}$
• Correction
• ${K_t} = {\overline P _t}H_t^T{({H_t}{\overline P _t}H_t^T + {Q_t})^{ - 1}}$
• ${x_t} = {\overline x _t} + {K_t}({z_t} - {H_t}{\overline x _t})$
• ${P_t} = (I - {K_t}{H_t}){\overline P _t}$

• 第一行基於轉移矩陣和控制輸入，預測$t$時刻的狀態
• 第二行是預測方差矩陣
• 第三行計算Kalman增益，K_t
• 第四行基於觀測的新息進行狀態更新
• 第五行計算更新狀態的方差矩陣。

可以看到算法的所有的精妙之處都在於第三行和第四行。我們可以這樣來理解：

1.    $({H_t}{\overline P _t}H_t^T + {Q_t})$代表對狀態進行觀測時，觀測的不確定程度，它與Kalman增益K_t成反比，表示觀測的可能噪聲越大的時候，Kalman增益K_t越小。
2. 再看第四行，${x_t}$的更新是在$\overline x_t$上加一個 $K_t$ 乘以 $({z_t} - {H_t}{\overline x _t})$。$({z_t} - {H_t}{\overline x _t})$代表的是預測的值與觀測之間的差異，這個差異當預測和觀測都比較接近於真實值時比較小。當觀測不准，或者預測不准時都會比較大。而前面的乘子K_t是在觀測噪聲大的時候比較小，所以整個${K_t}({z_t} - {H_t}{\overline x _t})$這個修正量，表示利用觀測對預測結果的修正量。
• 當觀測噪聲比較小，預測誤差比較大時修正幅度比較大
• 當觀測噪聲比較小預測誤差比較小的時候，或者觀測噪聲比較大的時候，修正誤差的幅度也比較小，從而起到了一種平滑的作用。
3. 利用較准確的觀測修正預測誤差，不准確的觀測修正量也較小，所以在誤差較大的時候能快速修正，而在誤差較小時能逐漸收斂。

2.4 Kalman濾波算法的推導

這里我們用Bayes公式，給出Kalman Filter是如何導出的。

1. 系統的初始狀態是：

$bel({x_0}) = N\left( {{\mu _0},{P_0}} \right)$

2. 預測過程的推導

狀態轉移模型是線性函數

${x_t} = {A_t}{x_{t - 1}} + {B_t}{u_t} + {\varepsilon _t}$

所以，由$x_{t-1}$到$x_{t}$狀態轉移的條件概率為:

$p({x_t}|{u_t},{x_{t - 1}}) = N\left( {{A_t}{x_{t - 1}} + {B_t}{u_t},{R_t}} \right)$

回顧Bayes公式，計算預測狀態的分布，需要考慮所有可能的$x_{t-1}$：

$\overline {bel} ({x_t}) = \int {p({x_t}|{u_t},{x_{t - 1}})} {\rm{ }}bel({x_{t - 1}})\;d{x_{t - 1}}$

這正是計算兩個高斯分布的卷積的過程，參考文獻[2]：

$\begin{array}{l}
\overline {bel} ({x_t}) = \int {p({x_t}|{u_t},{x_{t - 1}})} \quad \;\;\quad \quad \quad \quad bel({x_{t - 1}})\;d{x_{t - 1}}\\
\quad \quad \quad \quad \quad \quad  \Downarrow \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad  \Downarrow \\
\quad \quad \quad \sim N\left( {{A_t}{\mu _{t - 1}} + {B_t}{u_t},{R_t}} \right)\quad \quad \sim N\left( {{\mu _{t - 1}},{P_{t - 1}}} \right)\\
\quad \quad \quad \quad \quad \quad  \Downarrow \quad \quad \quad \quad \quad \quad \quad \\
\overline {bel} ({x_t}) = \eta \;\int {\exp \left\{ { - \frac{1}{2}{{({x_t} - {A_t}{x_{t - 1}} - {B_t}{u_t})}^T}R_t^{ - 1}({x_t} - {A_t}{x_{t - 1}} - {B_t}{u_t})} \right\}} \\
\quad \quad \quad \quad \;\quad \exp \left\{ { - \frac{1}{2}{{({x_{t - 1}} - {\mu _{t - 1}})}^T}P_{t - 1}^{ - 1}({x_{t - 1}} - {\mu _{t - 1}})} \right\}\;d{x_{t - 1}}\\
\overline {bel} ({x_t}) = \left\{ {\begin{array}{*{20}{c}}
{{{\bar \mu }_t} = {A_t}{\mu _{t - 1}} + {B_t}{u_t}}\\
{{{\overline P }_t} = {A_t}{P_{t - 1}}A_t^T + {R_t}}
\end{array}} \right.\quad \quad \quad \quad \quad \quad \quad
\end{array}$

所以Kalman濾波器的預測過程，正是基於兩個高斯分布的卷積計算得到的解析表達式。

3. 觀測更新過程的推導

觀測方程也是線性方程，並且噪聲是高斯噪聲

${z_t} = {H_t}{x_t} + {\delta _t}$

所以$p({z_t}|{x_t}) $的條件概率是高斯分布的線性變換計算：

$p({z_t}|{x_t}) = N\left( {{H_t}{x_t},{Q_t}} \right)$

再考慮貝葉斯公式的狀態更新步驟

$bel({x_t}) = \,\quad \eta \quad \,p({z_t}|{x_t})\overline {bel} ({x_t})$

這正是兩個高斯分布的乘積的問題，參考文獻[2]

$\begin{array}{l}
bel({x_t}) = \,\quad \eta \quad \,p({z_t}|{x_t})\quad \quad \quad \quad \quad \quad \overline {bel} ({x_t})\\
\quad \quad \quad \quad \quad \quad \quad \quad  \Downarrow \quad \quad \quad \quad \quad \quad \quad \quad  \Downarrow \\
\quad \quad \quad \quad \quad \sim N\left( {{z_t};{H_t}{x_t},{Q_t}} \right)\quad \quad \sim N\left( {{x_t};{{\overline \mu  }_t},{{\overline P }_t}} \right)\\
\quad \quad \quad \quad \quad \quad \quad \quad  \Downarrow \\
bel({x_t}) = \eta \;\exp \left\{ { - \frac{1}{2}{{({z_t} - {H_t}{x_t})}^T}Q_t^{ - 1}({z_t} - {H_t}{x_t})} \right\}\exp \left\{ { - \frac{1}{2}{{({x_t} - {{\bar \mu }_t})}^T}\bar P_t^{ - 1}({x_t} - {{\bar \mu }_t})} \right\}\\
\end{array}$

所以，基於求高斯變量乘積的分布的方法，可以導出結果仍然是高斯分布，用它的二階矩表示：

$bel({x_t}) = \left\{ {\begin{array}{*{20}{c}}
{{\mu _t} = {{\bar \mu }_t} + {K_t}({z_t} - {H_t}{{\bar \mu }_t})}\\
{{P_t} = (I - {K_t}{H_t}){{\overline P }_t}}
\end{array}} \right.\quad \quad {\rm{with    }}{K_t} = {\overline P _t}H_t^T{({H_t}{\overline P _t}H_t^T + {Q_t})^{ - 1}}$

所以狀態更新中的Kalman增益，均值和方差的更新公式，都是這樣導出的。

 

2.5 Kalman濾波算法的舉例

圖3和圖4通過一維高斯分布的例子，給出在預測和更新過程中狀態變量的概率密度分布是如何變化的。

圖3. 預測過程的舉例，藍色曲線表示$x_{t-1}$的pdf，紫色曲線表示$\overline x_t$的pdf.

 

圖4. 更新過程舉例，紫色為預測后的pdf, 黃色為更新后的pdf，青色為觀測的結果

從這個例子中可以值得注意的是，在預測部分高斯分布的卷積一般會使狀態估計的方差加大；在觀測部分高斯分布的乘積一般會將估計的方差收窄。

 

2.6 Kalman濾波的代碼實現

Kalman濾波算法可以非常方便的用矩陣計算方法實現，其迭代更新過程的Matlab實現的代碼僅有如下幾行：

 

2.7 Kalman濾波的效果示例

通過實現一個簡單的Kalman濾波器，我們可以直觀的看一下Kalman濾波器的提高跟蹤准確性的效果。

圖5. Kalman濾波器的實驗效果示例，其中紅色實線是真值；藍色點是觀測；綠色線是滑動平均的結果；紫色曲線是Kalman濾波的結果。

圖6. 比較Kalman濾波的跟蹤結果和滑動平均的跟蹤結果

 

圖6給出了直觀的跟蹤結果與真實值之間的最小二乘誤差的比較，課件Kalman濾波算法相比滑動平均等，提供了更高的跟蹤准確性。

 

2.8 Kalman濾波的算法特點

1. Kalman濾波計算快速，計算復雜度為$O(m^{2.376} + n^2)$，其中$m$是觀測的維數；$n$是狀態的個數。
2. 對於線性系統，零均值高斯噪聲的系統，Kalman是理論上無偏的，最優濾波器。
3. Kalman濾波在實際使用中，要注意參數$R$和$Q$的調節，這兩者實際上是相對的，表示更相信觀測還是更相信預測。具體使用時，$R$可以根據過程噪聲的幅度決定，然后$Q$可以相對$R$來給定。當更相信觀測時，把$Q$調小，不相信觀測時，把$Q$調大。
4. $Q$越大，表示越不相信觀測，這是系統狀態越容易收斂，對觀測的變化響應越慢。$Q$越小，表示越相信觀測，這時對觀測的變化響應快，但是越不容易收斂。

參考文獻

[1]. Sebastian Thrun, Wolfram Burgard, Dieter Fox, Probabilistic Robotics, 2002, The MIT Press.

[2]. P.A. Bromiley, Products and Convolutions of Gaussian Probability Density Functions, University of Manchester