Particle Filter

这篇文章数学公式推得有点儿长。。。。提前预警= =

贝叶斯滤波

假设有一个系统:

• 状态方程：$x_k=f_k(x_{k-1},v_{k-1})$，x为系统状态，v为过程噪声;
• 测量方程：$y_k=h_k(x_k,n_k)$，y为测量数据，n为测量噪声。

从贝叶斯理论的观点来看，状态估计问题（目标跟踪、信号滤波）就是根据之前一系列的已有数据$y_{1:k}$（后验知识）??????????递推的计算出当前状态$x_k$的可信度。这个可信度就是概率公式$p(x_k|y_{1:k})$，它需要通过预测和更新两个步骤来递推的计算。

• 预测：利用系统模型预测状态的先验概率密度，即用先验知识对未来的状态进行猜测，$p(x_k|x_{k-1})$;
• 更新：利用最新的测量值对先验概率密度进行修正，得到后验概率密度，也就是对之前的猜测进行修正。

在处理这些问题时，一般都先假设系统的状态转移服从一阶马尔科夫模型，即当前时刻的状态$x_k$只与上一个时刻的状态$x_{k-1}$有关。

为了进行递推，不妨假设已知k-1时刻的概率密度函数$p(x_{k-1}|y_{1:k-1})$

预测

由上一时刻的概率密度$p(x_{k-1}|y_{1:k-1})$得到$p(x_{k}|y_{1:k-1})$，即用前面1：k-1时刻的测量数据，预测状态$x_k$出现的概率。
推导：

$$\begin{equation} \begin{split} p(x_{k}|y_{1:k-1}) &= \int p(x_k,x_{k-1}|y_{1:k-1})dx_{k-1} \\ &= \int p(x_k|x_{k-1},y_{1:k-1})p(x_{k-1}|y_{1:k-1})dx_{k-1} &\ \ \ \ //\ 纯粹的贝叶斯推导 \\ &= \int p(x_k|x_{k-1})p(x_{k-1}|y_{1:k-1})dx_{k-1} &\ \ \ \ //\ 一阶马尔科夫过程假设 \end{split} \end{equation}$$

如果没有噪声，$x_k$完全由$x_{k-1}$计算得到，也就没由概率分布这个概念了，由于出现了噪声，所以$x(k)$不好确定，因此才产生了概率。

更新

上一步还只是预测，这里又多了k时刻的测量，对上面的预测再进行修正，就是滤波了。这里的后验概率也将是代入到下次的预测，形成递推。
推导：

$$\begin{equation} \begin{split} p(x_k|y_{1:k}) &= \frac{p(y_k|x_k,y_{1:k-1})p(x_k|y_{1:k-1})}{p(y_k|y_{1:k-1})} \\ \\ &= \frac{p(y_k|x_k)p(x_k|y_{1:k-1})}{p(y_k|y_{1:k-1})} & //\ y_k只与x_k有关，p(y_k|x_k)是似然函数 \end{split} \end{equation}$$

其中归一化常数

$$p(y_k|y_{1:k-1})=\int p(y_k|x_k)p(x_k|y_{1:k-1})dx_k$$

同理，观测数据也是常数，不确定性和概率产生于观测噪声。

蒙特卡洛采样

上面的推导过程中需要用到积分，这对于一般的非线性，非高斯系统，很难得到后验概率的解析解。为了解决这个问题，就得引进蒙特卡洛采样。

假设我们能从一个目标概率分布p(x)中采样到一系列的样本（粒子）$x_1,\dots,x_N$，（至于怎么生成服从$p(x)$分布的样本，这个问题先放一放），那么就能利用这些样本去估计这个分布的某些函数的期望值。

$$\begin{equation} \begin{split} E[f(x)] &= \int_a^b f(x)p(x)dx \\ Var[f(x)] &= E\{f(x)-E[f(x)]\}^2 = \int_a^b\{f(x)-E[f(x)]\}^2p(x)dx \end{split} \end{equation}$$

上面的式子其实都是计算期望的问题，只是被积分的函数不同。蒙特卡洛采样的思想就是用平均值来代替积分，求期望：

$$E(f(x))\approx \frac{f(x_1)+...+f(x_N)}{N}$$

假设可以从后验概率中采样到N个样本，那么后验概率的计算可表示为：

$$\hat{p}(x_n|y_{1:k})=\frac{1}{n}\sum_{i=1}^{N}\delta(x_n-x_n^{(i)})\approx p(x_n|y_{1:k}) \ \ \ \ //\ 其中\delta(x_n-x_n^{(i)})是狄拉克函数$$

用这些采样的粒子的状态值直接平均就得到了期望值，也就是滤波后的值，这里的$f(x)$就是每个粒子的状态函数。这就是粒子滤波了。

$$\begin{equation} \begin{split} E[f(x)] &\approx \int f(x_n)\hat{p}(x_n|y_{1:k})dx_n \\ &= \frac{1}{n}\sum_{i=1}{N}\int f(x_n)\delta(x_n-x_n^{(i)})dx_n \\ &= \frac{1}{n}\sum_{i=1}{N}f(x_n^{(i)}) \end{split} \end{equation}$$

重要性采样

思路看似简单，但是要命的是后验概率不知道，所以这样直接去应用是行不通的，这时候得引入重要性采样这个方法来解决这个问题。

无法从目标分布中采样，就从一个已知的可以采样的分布里去采样如 $q(x|y)$，这样上面的求期望问题就变成了：

$$\begin{equation} \begin{split} E[f(x_k)] &=\int f(x_k)\frac{p(x_k|y_{1:k})}{q(x_k|y_{1:k})}q(x_k|y_{1:k})dx_k \\ \\ &=\int f(x_k)\frac{p(y_{1:k}|x_k)p(x)}{p(y_{1:k})q(x_k|y_{1:k})}q(x_k|y_{1:k})dx_k \\ \\ &=\int f(x_k)\frac{W_k(x_k)}{p(y_{1:k})}q(x_k|y_{1:k})dx_k & //\ W_k(x_k)=\frac{p(y_{1:k}|x_k)p(x_k)}{q(x_k|y_{1:k})}\propto\frac{p(x_k|y_{1:k})}{q(x_k|y_{1:k})}\\ \\ &=\frac{1}{p(y_{1:k})}\int f(x_k)W_k(x_k)q(x_k|y_{1:k})dx_k & //\ p(y_{1:k})=\int p(y_{1:k}|x_k)p(x_k)dx_k \\ \\ &=\frac{\int f(x_k)W_k(x_k)q(x_k|y_{1:k})dx_k}{\int p(y_{1:k}|x_k)p(x_k)dx_k} \\ \\ &=\frac{\int f(x_k)W_k(x_k)q(x_k|y_{1:k})dx_k}{\int W_k(x_k)q(x_k|y_{1:k})dx_k} \\ \\ &=\frac{E_{q(x_k|y_{1:k})}[W_k(x_k)f(x_k)]}{E_{q(x_k|y_{1:k})}[W_k(x_k)]} \\ \\ &\approx\frac{\frac{1}{N}\sum_{i=1}^{N}W_k(x_k^{(i)})f(x_k^{(i)})}{\frac{1}{N}\sum_{i=1}^{N}W_k(x_k^{(i)})f(x_k^{(i)})} & //\ Sampling\ N\ samples\ \left\{ x_k^{(i)} \right\}\sim q(x_k|y_{1:k}) \\ \\ &=\sum_{i=1}^N\tilde{W_k}(x_k^{(i)})f(x_k^{(i)}) & //\ \tilde{W_k}(x_k^{(i)})=\frac{W_k(x_k{(i)})}{\sum_{i=1}^{N}W_K(x_k^{(i)})} \end{split} \end{equation}$$

到这里已经解决了不能从后验概率直接采样的问题，但是上面这种每个粒子的权重都直接计算的方法效率低，因为每增加一个采样，$p(x_k|y_{1:k})$都得重新计算，并且还不好计算这个式子。所以最佳的形式是能够以递推的方式去计算权重，避开计算$p(x_k|y_{1:k})$，这就是所谓的序贯重要性采样（SIS），粒子滤波的原型。

下面开始权重w递推形式的推导：假设重要性概率密度函数$q(x_{0:k}|y_{1:k})$，这里$x$的下标是0:k，也就是说粒子滤波是估计过去所有时刻的状态的后验。假设它可以分解为：

$$q(x_{0:k}|y_{1:k})=q(x_{0:k-1}|y_{1:k-1})q(x_k|x_{0:k-1},y_{1:k})$$

后验概率密度函数的递归形式可以表示为：

$$\begin{equation} \begin{split} p(x_{0:k}|Y_k) &=\frac{p(y_k|x_{0:k},Y_{k-1})p(x_{0:k}|Y_{k-1})}{p(y_k|Y_{k-1})} & //\ Y_k = y_{1:k} \\ \\ &=\frac{p(y_k|x_{0:k},Y_{k-1})p(x_k|x_{0:k-1},Y_{k-1})p(x_{0:k-1}|Y_{k-1})}{p(y_k|Y_{k-1})} \\ \\ &=\frac{p(y_k|x_k)p(x_k|x_{k-1})p(x_{0:k-1}|Y_{k-1})}{p(y_k|Y_{k-1})} \\ \\ &\propto p(y_k|x_k)p(x_k|x_{k-1})p(x_{0:k-1}|Y_{k-1}) \end{split} \end{equation}$$

上面这个式子和上一节贝叶斯滤波中后验概率的推导是一样的，只是之前的$x_k$变成了这里的$x_{0:k}$，就是这个不同，导致贝叶斯估计里需要积分，而这里后验概率的分解形式却不用积分。

粒子权值的递归形式可以表示为:

$$\begin{equation} \begin{split} w_k^{(i)} &\propto\frac{p(x_{0:k}^{(i)}|Y_k)}{q(x_{0:k}^{(i)}|Y_k)} \\ \\ &= \frac{p(y_k|x_k^{(i)})p(x_k^{(i)}|x_{k-1}^{(i)})p(x_{0:k-1}^{(i)}|Y_{k-1})}{q(x_k^{(i)}|x_{0:k-1}^{(i)},Y_k)q(x_{0:k-1}^{(i)}|Y_{k-1})} \\ \\ &= w_{k-1}^{(i)}\frac{p(y_k|x_k^{(i)})p(x_k^{(i)}|x_{k-1}^{(i)})}{q(x_k^{(i)}|x_{0:k-1}^{(i)},Y_k)} \end{split} \end{equation}$$

参考

Particle Filter Tutorial 粒子滤波：从推导到应用（一 二 三 四）
粒子滤波器 - 维基百科
粒子滤波理论 - 百度文库
怎样从实际场景上理解粒子滤波（Particle Filter）？- zhihu
机器人定位问题：https://www.cnblogs.com/21207-iHome/p/5237701.html