卡爾曼濾波系列——（三）粒子濾波（重寫）

1 簡介

儘管對於大多數濾波問題中，採用高斯近似的方式（例如卡爾曼濾波和擴展卡爾曼濾波方法）可以很好地解決。但是對於多模式或者狀態量是離散的這樣的濾波問題，高斯近似並不合適；而對於非線性問題，擴展卡爾曼濾波在做局部線性化的操作時，同樣損失了一定的精度，模型的非線性度越強，損失得越多。

在這樣的情況下，基於序貫重要性採樣方法的例子濾波（Partical Filter，PF）是一個有效的替代方法，它通過蒙特卡洛近似的方式獲得貝葉斯濾波問題中後驗分佈的解，從而達到濾波或者狀態估計的目的。

---

2 原理

2.1 貝葉斯推斷中的蒙特卡洛近似

在貝葉斯推斷中，包括貝葉斯濾波，主要的問題通常集中在後驗分佈的期望計算，如下所示：

${\bf{E}}[{\bf{f}}\left( {\bf{\theta }} \right)|{{\bf{z}}_{1:k}}]{\rm{ }} = \int {{\bf{f}}\left( {\bf{\theta }} \right)p\left( {{{\bf{\theta }}}|{{\bf{z}}_{1:k}}} \right)d{{\bf{\theta }}}}$

其中$\bf{f}$表示從$\mathbb{R}^{N}\rightarrow \mathbb{R}^{M}$的任意函數，而$p\left( {{{\bf{\theta }}}|{{\bf{z}}_{1:k}}} \right)$表示給定觀測${\bf{z}}_{1:k}$下狀態${\bf{\theta }}$的後驗概率密度函數。現在的問題是這樣的積分只有在一些特殊情況中會獲得閉合解，在多數情況下，需要用到數值方法求解，蒙特卡洛採樣就是常用的數值解法。

蒙特卡洛採樣指的是從後驗分佈中採樣後取平均以估計統計量，從而代替閉合解的求取。

根據後驗分佈$p\left( {{{\bf{\theta }}}|{{\bf{z}}_{1:k}}} \right)$取$S$個採樣${{\bf{\theta }}^{(s)}}$，${{\bf{\theta }}^{(s)}} \sim p\left( {{\bf{\theta }}|{{\bf{z}}_{1:k}}} \right),s = 1,...,S$，於是可以用下式估計期望

${\bf{E}}[{\bf{f}}\left( {\bf{\theta }} \right)|{{\bf{z}}_{1:k}}]{\rm{ }} \approx \frac{1}{S}\sum\limits_{s = 1}^S {{\bf{f}}\left( {{{\bf{\theta }}^{(s)}}} \right)}$

2.2 重要性採樣

然而，在實際情況中，$p\left( {{{\bf{\theta }}}|{{\bf{z}}_{1:k}}} \right)$取$S$個採樣${{\bf{\theta }}^{(s)}}$ 的方程形式複雜，幾乎不可能直接從該後驗分佈中獲得採樣，所以選擇使用重要性採樣（Importance Sampling, IS）分佈$\pi \left( {{\bf{\theta }}|{{\bf{z}}_{1:k}}} \right)$來近似後驗分佈，從該分佈中獲取採樣相對簡單。

${\bf{E}}[{\bf{f}}\left( {\bf{\theta }} \right)\left| {{{\bf{z}}_{1:k}}} \right.] = \int {\bf{f}} \left( {\bf{\theta }} \right)\frac{{p\left( {{\bf{\theta }}|{{\bf{z}}_{1:k}}} \right)}}{{{\bf{\pi }}\left( {{\bf{x}}|{{\bf{z}}_{1:k}}} \right)}}{\bf{\pi }}\left( {{\bf{\theta }}|{{\bf{z}}_{1:k}}} \right)d{\bf{\theta }}\\ = \int {\bf{f}} \left( {\bf{\theta }} \right)\frac{{p\left( {{{\bf{z}}_{1:k}}|{\bf{\theta }}} \right)p\left( {\bf{\theta }} \right)}}{{p\left( {{{\bf{z}}_{1:k}}} \right)\pi \left( {{\bf{\theta }}|{{\bf{z}}_{1:k}}} \right)}}\pi \left( {{\bf{\theta }}|{{\bf{z}}_{1:k}}} \right)d{\bf{\theta }}\\ = \frac{{\int {\bf{f}} \left( {\bf{\theta }} \right)\frac{{p\left( {{{\bf{z}}_{1:k}}|{\bf{\theta }}} \right)p\left( {\bf{\theta }} \right)}}{{\pi \left( {{\bf{\theta }}|{{\bf{z}}_{1:k}}} \right)}}\pi \left( {{\bf{\theta }}|{{\bf{z}}_{1:k}}} \right)d{\bf{\theta }}}}{{\int {\frac{{p\left( {{{\bf{z}}_{1:k}}|{\bf{\theta }}} \right)p\left( {\bf{\theta }} \right)}}{{\pi \left( {{\bf{\theta }}|{{\bf{z}}_{1:k}}} \right)}}} \pi \left( {{\bf{\theta }}|{{\bf{z}}_{1:k}}} \right)d{\bf{\theta }}}}$

如果我們用$\omega$表示$\frac{{p\left( {{{\bf{z}}_{1:k}}|{\bf{\theta }}} \right)p\left( {\bf{\theta }} \right)}}{{\pi \left( {{\bf{\theta }}|{{\bf{z}}_{1:k}}} \right)}}$，再利用蒙特卡洛方法，就能獲得下式：

${\bf{E}}[{\bf{f}}\left( {\bf{\theta }} \right)\left| {{{\bf{z}}_{1:k}}} \right.] = \frac{{\int {\bf{f}} \left( {\bf{\theta }} \right)\omega \pi \left( {{\bf{\theta }}|{{\bf{z}}_{1:k}}} \right)d{\bf{\theta }}}}{{\int \omega \pi \left( {{\bf{\theta }}|{{\bf{z}}_{1:k}}} \right)d{\bf{\theta }}}} \approx \frac{{\sum\limits_{s = 1}^S {\bf{f}} \left( {{{\bf{\theta }}^{\left( s \right)}}} \right){\omega ^{(s)}}}}{{\sum\limits_{s = 1}^S {{\omega ^{(s)}}} }}$

於是有了重要性採樣粒子濾波算法。給定觀測模型$p\left( {{{\bf{z}}_{1:k}}|{\bf{\theta }}} \right)$和先驗$p\left( {\bf{\theta }} \right)$，可以將後驗用重要性採樣近似如下：

1. 從重要性分佈採樣得到$S$個粒子：
   ${{\bf{\theta }}^{(s)}} \sim \pi \left( {{\bf{\theta }}|{{\bf{z}}_{1:k}}} \right),s = 1,...,S$

2. 計算未歸一化的粒子權重${\omega ^{*(s)}}$：
   ${\omega ^{*(s)}} = \frac{{p\left( {{{\bf{z}}_{1:k}}|{{\bf{\theta }}^{(s)}}} \right)p\left( {{{\bf{\theta }}^{(s)}}} \right)}}{{\pi \left( {{{\bf{\theta }}^{(s)}}|{{\bf{z}}_{1:k}}} \right)}}$
   然後歸一化權重得到
   ${\omega ^{(s)}} = \frac{{{\omega ^{*(s)}}}}{{\sum\limits_{i = 1}^S {{\omega ^{(i)}}} }}$

3. 於是${\bf{f}}\left( {\bf{\theta }} \right)$的後驗期望可以用下式近似
   ${\bf{E}}[{\bf{f}}\left( {\bf{\theta }} \right)\left| {{{\bf{z}}_{1:k}}} \right.] \approx \sum\limits_{s = 1}^S {\bf{f}} \left( {{{\bf{\theta }}^{\left( s \right)}}} \right){\omega ^{(s)}}$

上述算法所描述的後驗概率密度可以寫成如下形式：
$p\left( {{\bf{\theta }}|{{\bf{z}}_{1:k}}} \right) \approx \sum\limits_{s = 1}^S {{\omega ^{(s)}}\delta \left( {{\bf{\theta }} - {{\bf{\theta }}^{(s)}}} \right)}$

其中$\delta \left( \cdot \right)$表示狄利克雷函數。

2.3 序貫重要性採樣

序貫重要性採樣（Sequential importance sampling，SIS）是序貫版本的重要性採樣，SIS算法可用於生成重要性採樣近似值，以濾波形式如下的通用狀態空間模型的分佈，狀態轉移方程和觀測方程如下：

${{\bf{\theta }}_k} = {\bf{f}}\left( {{{\bf{\theta }}_{k - 1}}} \right) + {{\bf{s}}_k}$

${{\bf{z}}_k} = {\bf{h}}\left( {{{\bf{\theta }}_k}} \right) + {{\bf{v}}_k}$

其中${\bf{\theta }}_k$表示第${k}$時刻的系統狀態， ${\bf{z}}_k$表示對${k}$時刻系統狀態的觀測結果，而${\bf{f}}( \cdot )$和${\bf{h}}( \cdot )$分別代表狀態轉移函數和觀測函數，另外${\bf{s}}_k$爲狀態噪聲，${{\bf{s}}_k}\sim N\left( {{\bf{0}},{\bf{Q}}} \right)$， ${{\bf{r}}_k}$爲觀測噪聲，${{\bf{r}}_k}\sim N\left( {{\bf{0}},{\bf{R}}} \right)$。濾波問題在於如何利用傳感器的觀測${{\bf{z}}_k}$，實現對系統狀態${{\bf{\theta }}_k}$更準確的估計。

考慮給定觀測${{\bf{z}}_{1:k}}$的條件下所有狀態${{\bf{\theta }}_{0:k}}$的完整後驗分佈，利用模型的Markov性質，可以得到如下後驗分佈的遞推公式：

$p\left( {{{\bf{\theta }}_{0:k}}|{{\bf{z}}_{1:k}}} \right) \propto p\left( {{{\bf{z}}_k}|{{\bf{\theta }}_{0:k}},{{\bf{z}}_{1:k - 1}}} \right)p\left( {{{\bf{\theta }}_{0:k}}|{{\bf{z}}_{1:k - 1}}} \right)\\ = p\left( {{{\bf{z}}_k}|{{\bf{\theta }}_k},{{\bf{z}}_{1:k - 1}}} \right)p\left( {{{\bf{\theta }}_k}|{{\bf{\theta }}_{0:k - 1}},{{\bf{z}}_{1:k - 1}}} \right)p\left( {{{\bf{\theta }}_{0:k - 1}}|{{\bf{z}}_{1:k - 1}}} \right)\\ = p\left( {{{\bf{z}}_k}|{{\bf{\theta }}_k}} \right)p\left( {{{\bf{\theta }}_k}|{{\bf{\theta }}_{k - 1}}} \right)p\left( {{{\bf{\theta }}_{0:k - 1}}|{{\bf{z}}_{1:k - 1}}} \right)$

因此，粒子的重要性權重可以用下式計算

$\omega _k^{(s)} \propto \frac{{p\left( {{{\bf{z}}_k}|{\bf{\theta }}_k^{(s)}} \right)p\left( {{\bf{\theta }}_k^{(s)}|{\bf{\theta }}_{k - 1}^{(s)}} \right)p\left( {{\bf{\theta }}_{0:k - 1}^{(s)}|{{\bf{z}}_{1:k - 1}}} \right)}}{{\pi \left( {{\bf{\theta }}_{0:k}^{(s)}|{{\bf{z}}_{1:k}}} \right)}}$

如果構造的重要性採樣滿足下列關係

$\pi \left( {{{\bf{\theta }}_{0:k}}|{{\bf{z}}_{1:k}}} \right) = \pi \left( {{{\bf{\theta }}_k}|{{\bf{\theta }}_{0:k - 1}}{{\bf{z}}_{1:k}}} \right)\pi \left( {{{\bf{\theta }}_{0:k - 1}}|{{\bf{z}}_{1:k - 1}}} \right)$

那麼可以獲得以下粒子權重的遞推關係式

$\omega _k^{(s)} \propto \frac{{p\left( {{{\bf{z}}_k}|{\bf{\theta }}_k^{(s)}} \right)p\left( {{\bf{\theta }}_k^{(s)}|{\bf{\theta }}_{k - 1}^{(s)}} \right)}}{{\pi \left( {{\bf{\theta }}_k^{(s)}|{\bf{\theta }}_{0:k - 1}^{(s)},{{\bf{z}}_{1:k}}} \right)}}\frac{{p\left( {{\bf{\theta }}_{0:k - 1}^{(s)}|{{\bf{z}}_{1:k - 1}}} \right)}}{{\pi \left( {{\bf{\theta }}_{0:k - 1}^{(s)}|{{\bf{z}}_{1:k - 1}}} \right)}}\\ \propto \frac{{p\left( {{{\bf{z}}_k}|{\bf{\theta }}_k^{(s)}} \right)p\left( {{\bf{\theta }}_k^{(s)}|{\bf{\theta }}_{k - 1}^{(s)}} \right)}}{{\pi \left( {{\bf{\theta }}_k^{(s)}|{\bf{\theta }}_{0:k - 1}^{(s)},{{\bf{z}}_{1:k}}} \right)}}\omega _{k - 1}^{(s)}$

更進一步地，可以選取重要性採樣如下

$\pi \left( {{{\bf{\theta }}_k}|{{\bf{\theta }}_{0:k - 1}},{{\bf{z}}_{1:k}}} \right) = p\left( {{{\bf{\theta }}_k}|{{\bf{\theta }}_{k - 1}}} \right)$

那麼粒子權重的遞推關係式可簡化爲

$\omega _k^{(s)} \propto p\left( {{{\bf{z}}_k}|{\bf{\theta }}_k^{(s)}} \right)\omega _{k - 1}^{(s)}$

於是有了序貫重要性採樣粒子濾波算法如下：

1. 從$k$時刻的序貫重要性分佈採樣得到$S$個粒子：
   ${\bf{\theta }}_k^{(s)} \sim p\left( {{{\bf{\theta }}_k}|{\bf{\theta }}_{k - 1}^{(s)}} \right),s = 1,...,S$

2. 計算未歸一化的粒子權重${\omega ^{*(s)}}$：
   $\omega _k^{*(s)} = p\left( {{{\bf{z}}_k}|{\bf{\theta }}_k^{(s)}} \right)$
   然後歸一化權重得到
   $\omega _k^{(s)} = \frac{{\omega _k^{*(s)}}}{{\sum\limits_{i = 1}^S {\omega _k^{(i)}} }}$

3. 於是${\bf{f}}\left( {{{\bf{\theta }}_k}} \right)$的後驗期望可以用下式近似
   ${\bf{E}}[{\bf{f}}\left( {{{\bf{\theta }}_k}} \right)\left| {{{\bf{z}}_{1:k}}} \right.] \approx \sum\limits_{s = 1}^S {\bf{f}} \left( {{\bf{\theta }}_k^{\left( s \right)}} \right)\omega _k^{(s)}$

2.4 序貫重要性重採樣

上一節中描述的SIS算法中的一個問題是，我們很容易遇到幾乎所有粒子的權重爲零或接近零的情況。這在粒子過濾文獻中被稱爲簡併性問題，它阻止了粒子過濾器多年的實際應用。

退化問題可以通過使用重採樣過程來解決。它是指從權重定義的離散分佈中提取$S$個新樣本，並用該新集合替換$S$個樣本的舊集合的過程。此重採樣過程可以表示爲以下算法：

1. 將每個權重$\omega _k^{(s)}$作爲從集合$\left\{ {\theta _k^{(s)}:s = 1,...,S} \right\}$中抽到索引爲$s$粒子的概率；
2. 從該離散分佈中抽取$S$個樣本，並用新樣本集替換舊樣本集；
3. 把所有粒子的權重設置爲常數：$\omega _k^{(s)} = \frac{1}{S}$

基於以上重採樣方法，結合序貫重要性採樣粒子濾波，得到序貫重要性重採樣粒子濾波算法：

1. 從$k$時刻的序貫重要性分佈採樣得到$S$個粒子：
   ${\bf{\tilde \theta }}_k^{(s)} \sim p\left( {{{\bf{\theta }}_k}|{\bf{\theta }}_{k - 1}^{(s)}} \right),s = 1,...,S$

2. 計算未歸一化的粒子權重$\omega _k^*$：
   $\omega _k^{*(s)} = p\left( {{{\bf{z}}_k}|{\bf{\tilde \theta }}_k^{(s)}} \right)$
   然後歸一化權重得到
   $\tilde \omega _k^{(s)} = \frac{{\omega _k^{*(s)}}}{{\sum\limits_{i = 1}^S {\omega _k^{(i)}} }}$

3. 從原始粒子集合$\left\{ {{\bf{\tilde \theta }}_k^{(s)},{{\tilde \omega }^{(s)}}} \right\},s = 1,...,S$中重採樣，獲得新的權重均爲$\frac{1}{S}$的粒子集合爲$\left\{ {{\bf{\theta }}_k^{(s)}} \right\},s = 1,...,S$；

4. 於是${\bf{f}}\left( {{{\bf{\theta }}_k}} \right)$的後驗期望可以用下式近似
   ${\bf{E}}[{\bf{f}}\left( {{{\bf{\theta }}_k}} \right)\left| {{{\bf{z}}_{1:k}}} \right.] \approx \frac{1}{S}\sum\limits_{s = 1}^S {\bf{f}} \left( {{\bf{\theta }}_k^{\left( s \right)}} \right)$

3 實驗

假設一輛車子在空曠場地上行駛，通過毫米波雷達測量車子實時相對於雷達的距離、方位角和徑向速度，之後通過該觀測結果，估計該車輛的實時狀態（包括相對橫縱座標、相對橫縱向速度）。

車子的實時狀態用${\left[ {x,y,\dot x,\dot y} \right]^T}$表示，觀測量用${\left[ {r,\theta ,v} \right]^T}$表示，有以下狀態轉移方程和觀測方程：

${\begin{bmatrix} {{x_k}}\\ {{y_k}}\\ {{{\dot x}_k}}\\ {{{\dot y}_k}} \end{bmatrix}} = \bf{f}({\begin{bmatrix} {{x_{k - 1}}}\\ {{y_{k - 1}}}\\ {{{\dot x}_{k - 1}}}\\ {{{\dot y}_{k - 1}}} \end{bmatrix}}) + {{\bf{s}}_k} = {\begin{bmatrix} 1&0&{\Delta t}&0\\ 0&1&0&{\Delta t}\\ 0&0&1&0\\ 0&0&0&1 \end{bmatrix}} {\begin{bmatrix} {{x_{k - 1}}}\\ {{y_{k - 1}}}\\ {{{\dot x}_{k - 1}}}\\ {{{\dot y}_{k - 1}}} \end{bmatrix}} + {{\bf{s}}_k}$

${\begin{bmatrix} {{r_k}}\\ {{\theta _k}}\\ {{v_k}} \end{bmatrix}} = h( {\begin{bmatrix} {{x_k}}\\ {{y_k}}\\ {{{\dot x}_k}}\\ {{{\dot y}_k}} \end{bmatrix}}) + {{\bf{v}}_k} = {\begin{bmatrix} {\sqrt {x_k^2 + x_y^2} }\\ {\arctan \frac{{{y_k}}}{{{x_k}}}}\\ {\frac{{{x_k}{{\dot x}_k} + {y_k}{{\dot y}_k}}}{{\sqrt {x_k^2 + x_y^2} }}} \end{bmatrix}}$

給定毫米波雷達觀測目標距離的噪聲均值爲0，方差爲10（單位米），觀測方位角的噪聲均值爲0，方差爲0.001（單位弧度），測量徑向速度的噪聲均值爲0，方差爲2（單位m/s）；採樣時間點爲500個，採樣的時間間隔 $\Delta t = 0.01$秒。

車輛的初始狀態爲${\left[ {{\rm{ - 20,20,10,10}}} \right]^T}$，四個狀態量的噪聲的方差分別爲$\left[ {{\rm{0}}{\rm{.01,0}}{\rm{.01,0}}{\rm{.05,0}}{\rm{.05}}} \right]$。仿真結果圖1和圖2所示：

4 總結

可以看到，相對於直接取用雷達觀測得到的距離和方位角換算出結果作爲目標狀態的估計而言，通過粒子濾波後獲得的狀態估計要更加準確，狀態波動小，較爲穩定，更符合實際中車輛的行駛過程。

本次實驗中，通過擴展卡爾曼濾波技術確定的定位均方誤差約爲0.17m2，而直接用觀測結果的定位均方誤差爲11.1m2，可以看出粒子濾波在定位精度上可以帶來極大地提升。

基於序貫重要性重採樣的粒子濾波算法的核心在於利用蒙特卡洛方法求解後驗期望，其重要性採樣選取狀態空間的轉移概率，使得粒子的遞推求解過程非常簡便，並做了粒子的重採樣，保證了粒子的高效性，也進一步地簡化粒子的遞推過程。

在給定的狀態空間轉移和觀測模型下，粒子濾波算法的主要思想體現在通過若干粒子模擬系統的真實狀態，如果某粒子的觀測結果與實際觀測很接近，那麼該粒子就會獲得高權重，反之粒子獲得低權重。之後通過重採樣的方式篩除低權重粒子，保留高權重的粒子。最後所有粒子共同決定對當前時刻系統狀態的估計結果，能夠起到抑制噪聲的目的。

相比於擴展卡爾曼濾波，粒子濾波通過蒙特卡洛採樣的方式近似後驗分佈，而不用做線性化，因此不會因爲線性化過程導致精度損失，可以獲得更準確的狀態估計效果。

5 參考文獻

[1] Simo Srkk. Bayesian Filtering and Smoothing.

---

原創性聲明：本文屬於作者原創性文章，小弟碼字辛苦，轉載還請註明出處。謝謝~

如果有哪些地方表述的不夠得體和清晰，有存在的任何問題，亦或者程序存在任何考慮不周和漏洞，歡迎評論和指正，謝謝各路大佬。

有需要相關技術支持的可諮詢QQ：297461921