今天我們來講一下粒子濾波算法，這也是我在學習過程中的一個筆記，由於有很多的個人理解，有不對的地方歡迎大家批評指正。
另：個人博客 Glooow 開業啦！歡迎各位大駕光臨
貝葉斯濾波是我們理解粒子濾波的基礎。假設我們有如下圖的隱馬爾科夫模型(Hidden Markov Model)，並且有如下的系統方程與觀測方程，我們只有觀測值 y 1 : T \boldsymbol{y}_{1:T} y 1 : T x 1 : T \boldsymbol{x}_{1:T} x 1 : T 濾波 所要做的事情。System Model : x k = f k ( x k − 1 , v k − 1 ) Observation Model : y k = h k ( x k , n k )
\begin{aligned}
\text{System Model}&: x_{k}=f_{k}\left(x_{k-1}, v_{k-1}\right) \\
\text{Observation Model}&: \mathrm{y}_{k}=h_{k}\left(\mathrm{x}_{k}, \mathrm{n}_{k}\right)
\end{aligned}
System Model Observation Model : x k = f k ( x k − 1 , v k − 1 ) : y k = h k ( x k , n k )
假設我們現在已經處於 t k − 1 t_{k-1} t k − 1 p ( x k − 1 ∣ y 1 : k − 1 ) p({x}_{k-1}|\boldsymbol{y}_{1:k-1}) p ( x k − 1 ∣ y 1 : k − 1 ) k − 1 k-1 k − 1 y 1 : k − 1 \boldsymbol{y}_{1:k-1} y 1 : k − 1 k − 1 k-1 k − 1 x k − 1 x_{k-1} x k − 1 後驗概率 分佈，也即 p ( x k − 1 ∣ y 1 : k − 1 ) p({x}_{k-1}|\boldsymbol{y}_{1:k-1}) p ( x k − 1 ∣ y 1 : k − 1 ) k k k y k y_k y k p ( x k ∣ y 1 : k ) p({x}_{k}|\boldsymbol{y}_{1:k}) p ( x k ∣ y 1 : k ) 預測 和更新 兩個階段來估計，下面我就解釋一下這兩個階段的含義。
前面說了我們有系統模型和觀測模型，首先根據系統模型，我們有了 k − 1 k-1 k − 1 x k − 1 x_{k-1} x k − 1 x k x_k x k x k x_k x k 預測(Update) 。通過預測階段，我們可以獲得先驗概率 分佈(注意這裏我們將 p ( x k ∣ y 1 : k − 1 ) p({x}_k|\boldsymbol{y}_{1:k-1}) p ( x k ∣ y 1 : k − 1 ) p ( x k ∣ y 1 : k − 1 ) = ∫ p ( x k ∣ x k − 1 ) p ( x k − 1 ∣ y 1 : k − 1 ) d x k − 1
\begin{aligned}p({x}_k|\boldsymbol{y}_{1:k-1})=\int p({x}_k|x_{k-1})p(x_{k-1}|\boldsymbol{y}_{1:k-1})dx_{k-1}\end{aligned}
p ( x k ∣ y 1 : k − 1 ) = ∫ p ( x k ∣ x k − 1 ) p ( x k − 1 ∣ y 1 : k − 1 ) d x k − 1 x k x_k x k y k y_k y k 更新 階段。
有了先驗 ，又有了觀測，根據貝葉斯公式，我們可以很容易得到後驗概率 分佈 p ( x k ∣ y 1 : k ) p\left(x_{k} | \boldsymbol{y}_{1: k}\right) p ( x k ∣ y 1 : k ) p ( y k ∣ x k ) p\left(y_{k} | x_{k}\right) p ( y k ∣ x k ) y k y_k y k x k x_k x k 似然函數 對先驗概率 進行加權 就得到了後驗概率 。p ( x k ∣ y 1 : k ) = p ( y k ∣ x k , y 1 : k − 1 ) p ( x k ∣ y 1 : k − 1 ) p ( y k ∣ y 1 : k − 1 ) ∝ p ( y k ∣ x k , y 1 : k − 1 ) p ( x k ∣ y 1 : k − 1 ) = p ( y k ∣ x k ) p ( x k ∣ y 1 : k − 1 )
\begin{aligned}
p\left(x_{k} | \boldsymbol{y}_{1: k}\right)&=\frac{p\left(y_{k} | x_{k},\boldsymbol{y}_{1: k-1}\right) p\left(x_{k} | \boldsymbol{y}_{1: k-1}\right)}{p\left(y_{k} | \boldsymbol{y}_{1: k-1}\right)} \\
&\propto p\left(y_{k} | x_{k},\boldsymbol{y}_{1: k-1}\right) p\left(x_{k} | \boldsymbol{y}_{1: k-1}\right) \\
&= p\left(y_{k} | x_{k}\right) p\left(x_{k} | \boldsymbol{y}_{1: k-1}\right)
\end{aligned}
p ( x k ∣ y 1 : k ) = p ( y k ∣ y 1 : k − 1 ) p ( y k ∣ x k , y 1 : k − 1 ) p ( x k ∣ y 1 : k − 1 ) ∝ p ( y k ∣ x k , y 1 : k − 1 ) p ( x k ∣ y 1 : k − 1 ) = p ( y k ∣ x k ) p ( x k ∣ y 1 : k − 1 )
怎麼理解這個加權呢？舉個栗子
比如
總結起來，我們濾波分爲兩個階段Prediction : p ( x k ∣ y 1 : k − 1 ) = ∫ p ( x k ∣ x k − 1 ) p ( x k − 1 ∣ y 1 : k − 1 ) d x k − 1 Update : p ( x k ∣ y 1 : k ) ∝ p ( y k ∣ x k ) p ( x k ∣ y 1 : k − 1 )
\begin{aligned}
\text{Prediction}&: p({x}_k|\boldsymbol{y}_{1:k-1})=\int p({x}_k|x_{k-1})p(x_{k-1}|\boldsymbol{y}_{1:k-1})dx_{k-1} \\
\text{Update}&: p\left(x_{k} | \boldsymbol{y}_{1: k}\right)\propto p\left(y_{k} | x_{k}\right) p\left(x_{k} | \boldsymbol{y}_{1: k-1}\right)
\end{aligned}
Prediction Update : p ( x k ∣ y 1 : k − 1 ) = ∫ p ( x k ∣ x k − 1 ) p ( x k − 1 ∣ y 1 : k − 1 ) d x k − 1 : p ( x k ∣ y 1 : k ) ∝ p ( y k ∣ x k ) p ( x k ∣ y 1 : k − 1 ) 積分 ，這在實際當中往往是很難計算的，而對於那些非常規的PDF，甚至不能給出解析解。解決這種問題一般有兩種思路：
建立簡單的模型，獲得解析解，如卡爾曼濾波(Kalman Filter)及EKF、UKF等；
建立複雜的模型，獲得近似解，如粒子濾波(Particle Filter)等。
卡爾曼濾波的思路就是將系統建模線性模型 ，隱變量、控制變量、控制噪聲與觀測噪聲均爲高斯分佈，那麼觀測變量也是隨機變量，整個模型中所有的隨機變量都是高斯 的！高斯分佈是我們最喜歡的分佈，因爲在前面貝葉斯濾波的預測階段 我們可以不用積分了，只需要計算均值和協方差就可以了！根據系統模型和觀測模型，我們可以獲得濾波的閉式解，這就是卡爾曼濾波的思想。System Model : x k = A x k − 1 + B u k − 1 + v k − 1 Observation Model : y k = C x k + n k
\begin{aligned}
\text{System Model}&: \mathrm{x}_{k}=A\mathrm{x}_{k-1} + B\mathrm{u}_{k-1} + v_{k-1} \\
\text{Observation Model}&: \mathrm{y}_{k}=C\mathrm{x}_{k}+ \mathrm{n}_{k}
\end{aligned}
System Model Observation Model : x k = A x k − 1 + B u k − 1 + v k − 1 : y k = C x k + n k
粒子濾波是什麼是意思呢？我們可以用大量的採樣值來描述一個概率分佈，當我們按照一個概率分佈進行採樣的時候，某個點的概率密度越高，這個點被採到的概率越大，當採樣數目足夠大(趨於無窮)的時候，粒子出現的頻率就可以用來表示對應的分佈，實際中我們可以理解一個粒子就是一個採樣。
但是對於離散型 的隨機變量還好，可以很容易的求出來狀態空間中每個狀態的頻率。但如果對於連續分佈 ，其實是很不方便的，所幸實際中我們也不需要獲得概率分佈的全部信息，一般只需要求出期望 就可以了。
下面爲了公式書寫和推導方便，以離散型隨機變量爲例，假設狀態空間爲 Z = { 1 , 2 , . . . , K } \mathcal{Z}=\{1,2,...,K\} Z = { 1 , 2 , . . . , K } N N N z i ∈ Z z_i \in \mathcal{Z} z i ∈ Z q ( z ) q(\mathbf{z}) q ( z ) N N N z 1 : N \boldsymbol{z}_{1:N} z 1 : N 經驗分佈 p ^ ( b ∣ z 1 : N ) = 1 N ∑ i I ( b − z i ) \hat{p}(b|\boldsymbol{z}_{1:N}) = \frac{1}{N}\sum_i \mathbb{I}(b-z_i) p ^ ( b ∣ z 1 : N ) = N 1 ∑ i I ( b − z i ) I ( ⋅ ) \mathbb{I}(\cdot) I ( ⋅ ) N N N p ^ ( b ∣ z 1 : N ) \hat{p}(b|\boldsymbol{z}_{1:N}) p ^ ( b ∣ z 1 : N ) q ( z ) q(\mathbf{z}) q ( z ) t = g ( z ) \mathsf{t}=g(\mathsf{z}) t = g ( z ) E [ t ] = E [ g ( z ) ] = ∑ b ∈ Z g ( b ) q ( b ) ≈ ∑ b g ( b ) p ^ ( b ∣ z 1 : N ) = ∑ b g ( b ) 1 N ∑ i I ( b − z i ) = 1 N ∑ i g ( z i )
\begin{aligned}
\mathbb{E}[\mathsf{t}] &= \mathbb{E}[g(\mathsf{z})]=\sum_{b\in\mathcal{Z}}g(b)q(b) \\
&\approx \sum_b g(b)\hat{p}(b|\boldsymbol{z}_{1:N}) \\
&= \sum_b g(b)\frac{1}{N}\sum_i \mathbb{I}(b-z_i)\\
&= \frac{1}{N}\sum_i g(z_i)
\end{aligned}
E [ t ] = E [ g ( z ) ] = b ∈ Z ∑ g ( b ) q ( b ) ≈ b ∑ g ( b ) p ^ ( b ∣ z 1 : N ) = b ∑ g ( b ) N 1 i ∑ I ( b − z i ) = N 1 i ∑ g ( z i ) 簡單求和 就可以了！連續型隨機變量也是類似的，所以在後面的粒子濾波中，我們利用粒子估計了概率密度分佈，要想求期望就只需要進行求和平均就可以了。
好了，有了前面的預備知識，我們可以先看一個粒子濾波的簡單例子。
假設我們現在有采樣好的 N N N { x k − 1 i } i = 1 N \{x_{k-1}^i\}_{i=1}^N { x k − 1 i } i = 1 N p ( x k − 1 ∣ y 1 : k − 1 ) p({x}_{k-1}|\boldsymbol{y}_{1:k-1}) p ( x k − 1 ∣ y 1 : k − 1 ) 預測 和更新 階段呢？
回顧一下預測的公式Prediction : p ( x k ∣ y 1 : k − 1 ) = ∫ p ( x k ∣ x k − 1 ) p ( x k − 1 ∣ y 1 : k − 1 ) d x k − 1
\text{Prediction}: p({x}_k|\boldsymbol{y}_{1:k-1})=\int p({x}_k|x_{k-1})p(x_{k-1}|\boldsymbol{y}_{1:k-1})dx_{k-1} \\
Prediction : p ( x k ∣ y 1 : k − 1 ) = ∫ p ( x k ∣ x k − 1 ) p ( x k − 1 ∣ y 1 : k − 1 ) d x k − 1 x k − 1 i x_{k-1}^i x k − 1 i 進化 ”一步，也就是說按照分佈 p ( x k ∣ x k − 1 ) p(x_k|x_{k-1}) p ( x k ∣ x k − 1 ) x k i ∼ p ( x k ∣ x k − 1 i ) x_k^i \sim p(x_k|x_{k-1}^i) x k i ∼ p ( x k ∣ x k − 1 i ) N N N { x k i } i = 1 N \{x_k^i\}_{i=1}^N { x k i } i = 1 N { x k − 1 i } i = 1 N \{x_{k-1}^i\}_{i=1}^N { x k − 1 i } i = 1 N p ( x k − 1 ∣ y 1 : k − 1 ) p({x}_{k-1}|\boldsymbol{y}_{1:k-1}) p ( x k − 1 ∣ y 1 : k − 1 ) { x k i } i = 1 N \{x_k^i\}_{i=1}^N { x k i } i = 1 N p ( x k ∣ y 1 : k − 1 ) p({x}_k|\boldsymbol{y}_{1:k-1}) p ( x k ∣ y 1 : k − 1 )
這樣做的好處是什麼呢？我們避免了積分 ！只需要對一個已知的分佈 p ( x k ∣ x k − 1 i ) p(x_k|x_{k-1}^i) p ( x k ∣ x k − 1 i )
通過預測我們獲得了 x k i ∼ p ( x k ∣ x k − 1 i ) x_k^i \sim p(x_k|x_{k-1}^i) x k i ∼ p ( x k ∣ x k − 1 i ) N N N 先驗概率 分佈。接下來就是更新，再回顧一下更新的公式Update : p ( x k ∣ y 1 : k ) ∝ p ( y k ∣ x k ) p ( x k ∣ y 1 : k − 1 )
\text{Update}: p\left(x_{k} | \boldsymbol{y}_{1: k}\right)\propto p\left(y_{k} | x_{k}\right) p\left(x_{k} | \boldsymbol{y}_{1: k-1}\right)
Update : p ( x k ∣ y 1 : k ) ∝ p ( y k ∣ x k ) p ( x k ∣ y 1 : k − 1 ) 似然函數 對先驗概率 進行加權 就得到了後驗概率 。如果簡單的把 x k i x_k^i x k i p ( x k i ∣ y 1 : k ) ∝ p ( y k ∣ x k i ) p ( x k i ∣ y 1 : k − 1 )
p\left(x_{k}^i | \boldsymbol{y}_{1: k}\right)\propto p\left(y_{k} | x_{k}^i\right) p\left(x_{k}^i | \boldsymbol{y}_{1: k-1}\right)
p ( x k i ∣ y 1 : k ) ∝ p ( y k ∣ x k i ) p ( x k i ∣ y 1 : k − 1 ) 粒子 有不同的權重 。
這裏怎麼理解呢？想一下前面提到的經驗分佈 ，我們只需要用粒子出現的頻率 來表示對應的概率，實際上就是在統計頻率的時候每個粒子的權重都是相同的，出現一次計數就加一。
而這裏的區別是什麼呢？由於我們有一個觀測值 y k y_k y k y k y_k y k i 1 i_1 i 1 y k y_k y k
那麼問題來了，前面我們說了濾波過程中要想求期望，只需要簡單求和就可以了E [ t ] = 1 N ∑ i g ( z i )
\mathbb{E}[\mathsf{t}] = \frac{1}{N}\sum_i g(z_i)
E [ t ] = N 1 i ∑ g ( z i ) E [ t ] = ∑ i w i ∑ j w j g ( z i )
\mathbb{E}[\mathsf{t}] = \sum_i \frac{w_i}{\sum_j w_j}g(z_i)
E [ t ] = i ∑ ∑ j w j w i g ( z i )
前面只講了一次預測和一次更新的過程，注意到我們前面只是從 k − 1 k-1 k − 1 k k k
不過細心的朋友們可能會覺得不太對勁，前面一個階段的例子裏，預測之前我們有采樣 { x k − 1 i } i = 1 N \{x_{k-1}^i\}_{i=1}^N { x k − 1 i } i = 1 N
這樣我們就可以得到一個完整的粒子濾波算法了！但是還有問題！
但是呢，有人發現了上面的算法不好啊！不是因爲麻煩，而是濾波到最後會發現只有少數一部分粒子有很大的權重，而其他絕大部分粒子的權重幾乎爲 0，這就意味着對於那些“不重要”的粒子，他們在我們求期望的時候貢獻並不大，但是我們卻花費了大量的計算精力來維護這些粒子，這樣不好不好！於是就有人提出了重採樣 。
重採樣什麼意思呢？你們粒子的權重不是不同嗎，那我就採取手段給你們整相同了！簡單理解，有兩個粒子，粒子 a a a b b b a a a b b b a a a 0.8 0.8 0 . 8
至此，加上重採樣我們就獲得了一個完整的比較好用的粒子濾波算法
但是還有一個問題，就是前面我們直接說有 N N N x k − 1 i ∼ p ( x k − 1 ∣ y 1 : k − 1 ) x_{k-1}^i \sim p({x}_{k-1}|\boldsymbol{y}_{1:k-1}) x k − 1 i ∼ p ( x k − 1 ∣ y 1 : k − 1 ) p ( x 1 ∣ y 1 ) p(x_1|y_1) p ( x 1 ∣ y 1 )
假如我們現在有另一個分佈 q ( x 1 : k ∣ y 1 : k ) q(\mathbf{x}_{1:k}|\mathbf{y}_{1:k}) q ( x 1 : k ∣ y 1 : k ) x k − 1 i ∼ q ( x 1 : k ∣ y 1 : k ) x_{k-1}^i \sim q(\mathbf{x}_{1:k}|\mathbf{y}_{1:k}) x k − 1 i ∼ q ( x 1 : k ∣ y 1 : k ) p ( x 0 : k ∣ y 1 : k ) p(\mathbf{x}_{0:k}|\mathbf{y}_{1:k}) p ( x 0 : k ∣ y 1 : k ) p ( x 0 : k ∣ y 1 : k ) ≈ ∑ i = 1 N w k i δ ( x 0 : k − x 0 : k i ) w k i ∝ p ( x 0 : k i ∣ y 1 : k ) q ( x 0 : k i ∣ y 1 : k )
\begin{aligned}
p(\mathbf{x}_{0:k}|\mathbf{y}_{1:k})&\approx \sum_{i=1}^{N}w_k^i\delta(\mathbf{x}_{0:k}-\mathbf{x}_{0:k}^i) \\
w_{k}^{i} &\propto \frac{p\left(\mathbf{x}_{0: k}^{i} | \mathbf{y}_{1: k}\right)}{q\left(\mathbf{x}_{0: k}^{i} | \mathbf{y}_{1: k}\right)}
\end{aligned}
p ( x 0 : k ∣ y 1 : k ) w k i ≈ i = 1 ∑ N w k i δ ( x 0 : k − x 0 : k i ) ∝ q ( x 0 : k i ∣ y 1 : k ) p ( x 0 : k i ∣ y 1 : k ) 預測 和更新 兩個遞推進行的階段，就有q ( x 0 : k ∣ y 1 : k ) = q ( x k ∣ x 0 : k − 1 , y 1 : k ) q ( x 0 : k − 1 ∣ y 1 : k − 1 ) = q ( x k ∣ x k − 1 , y k ) q ( x 0 : k − 1 ∣ y 1 : k − 1 ) w k i ∝ w k − 1 i p ( y k ∣ x k i ) p ( x k i ∣ x k − 1 i ) q ( x k i ∣ x k − 1 i , y k )
\begin{aligned}q\left(\mathbf{x}_{0: k} | \mathbf{y}_{1: k}\right)&=q\left(\mathbf{x}_{k} | \mathbf{x}_{0: k-1}, \mathbf{y}_{1: k}\right) q\left(\mathbf{x}_{0: k-1} | \mathbf{y}_{1: k-1}\right) \\
&= q\left(\mathbf{x}_{k} | \mathbf{x}_{k-1}, \mathbf{y}_{ k}\right) q\left(\mathbf{x}_{0: k-1} | \mathbf{y}_{1: k-1}\right) \\
w_{k}^{i} &\propto w_{k-1}^{i} \frac{p\left(\mathbf{y}_{k} | \mathbf{x}_{k}^{i}\right) p\left(\mathbf{x}_{k}^{i} | \mathbf{x}_{k-1}^{i}\right)}{q\left(\mathbf{x}_{k}^{i} | \mathbf{x}_{k-1}^{i}, \mathbf{y}_{k}\right)}
\end{aligned}
q ( x 0 : k ∣ y 1 : k ) w k i = q ( x k ∣ x 0 : k − 1 , y 1 : k ) q ( x 0 : k − 1 ∣ y 1 : k − 1 ) = q ( x k ∣ x k − 1 , y k ) q ( x 0 : k − 1 ∣ y 1 : k − 1 ) ∝ w k − 1 i q ( x k i ∣ x k − 1 i , y k ) p ( y k ∣ x k i ) p ( x k i ∣ x k − 1 i )
這篇文章主要是自己學習粒子濾波之後的一些理解，主要參考了文章 A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking ，但是這篇文章中是從 general 的情況開始講，然後舉了一些常用的特例，個人感覺不利於初學者理解，因此本文寫作過程中的思路是從簡單的特例開始再到更一般的情況。
最後一部分 [粒子濾波原理推導](## 粒子濾波原理推導) 其實寫的並沒有很清楚，主要是因爲自己太懶，到最後懶得一個個手敲公式了，如果想更清楚地瞭解其中的細節，可以閱讀上面那篇論文。
M. S. Arulampalam, S. Maskell, N. Gordon and T. Clapp, “A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian tracking,” in IEEE Transactions on Signal Processing , vol. 50, no. 2, pp. 174-188, Feb. 2002.
