- Introduction
- Method
- 动态系统的状态空间模型
- Bayes 滤波理论
- 重要性采样
- SIS算法
- SIR算法
- Resample算法
- Result&Discussion
- References
Introduction
估计理论的发展
从1795年高斯(K.Gauss)提出最小二乘估计法开始，估计理论及其方法经历了一个由低级到高级不断发展的过程。最小二乘估计法不考虑观测信号的统计特性，仅保证测量误差的方差最小，一般情况下其估计性能较差。
1942年Weiner提出了充分利用输入信号和观测信号统计特性的维纳滤波方法，它是一种线性最小方差滤波方法。维纳滤波是一种非递推的频域方法，不便于实时应用。
1960 年卡尔曼(R.E.Kalman)提出了适用于高斯线性系统状态的递推时域滤波方法即卡尔曼滤波，它是高斯线性系统的最优滤波器。
1969年 Bucy等人提出了可用于非线性系统的次优滤波器即扩展卡尔曼滤波（EKF），将卡尔曼滤波理论进一步推广到非线性领域。从此 EKF 成为非线性系统估计领域的经典方法。EKF 的基本思想是采用参数化的解析形式对系统模型的非线性进行线性近似，基于高斯假设用 Bayes 估计原理进行估计。
以上提出的这些估计方法，都属于Bayes估计，在Bayes估计下，又分最优估计和次优估计。通常的估计准则和方法有：最小二乘估计、极大似然估计、极大验后估计、最小方差估计、线性最小方差估计等
- 最优估计包括：Kalman滤波，网格(Grid)方法，Bene和Damu滤波；最优估计是指按照某种最优准则来估计未知量的值。
- 次优估计：实际问题因为面临大量非线性非高斯问题，最优估计很难找到解析解,所以用次优或逼近算法，可分为四大类：
- 解析逼近方法：包括扩展卡尔曼滤波(Extended Kalman Filter，EKF)，高阶EKF，迭代EKF，其共性如上所说：对非线性进行线性近似。
- 数值逼近方法：也称基于网格的逼近方法，它将连续状态空间分解为若干个网格单元，利用离散变量求和来代替积分来逼近状态后验分布，为了获得较好的逼近效果，要求网格必须足够密集，这在状态空间维数较高时导致计算量剧增。隐马尔科夫模型(Hidden Markov Model，HMM)是此种方法的代表，广泛应用于语音处理。
- 高斯和逼近方法：针对状态后验分布无法用单个高斯分布有效逼近的问题（例如当后验分布多峰时），高斯和逼近方法采用多个高斯分布的加权和来逼近。该类方法的主要困难在于混合个数的合理选择及混合个数可能随时间呈指数增加。
- 采样方法：采样方法使用一组样本来逼近状态后验分布，其典型代表有unscented Kalman滤波（UKF）和粒子滤波（Particle Filter，PF）。
粒子滤波的背景介绍
好了，上面介绍的最后，主角终于登场了，大概也知道为啥会提出粒子滤波了，因为理想很丰满，现实太骨感，哪有那么多线性高斯的问题呢？实际上还是非线性非高斯多啊，先简要介绍PF算法，如下：
粒子滤波使用了大量随机样本，采用用蒙特卡罗仿真(Monte Carlo Simulation)来完成递推贝叶斯滤波(Recursive Bayesian Filter)过程，其核心是使用一组具有相应权值的随机样本（粒子）来表示状态的后验分布。该方法的基本思路是选取一个重要性概率密度并从中进行随机抽样，得到一些带有相应权值的随机样本后，在状态观测的基础上调节权值的大小和粒子的位置，再使用这些样本来逼近状态后验分布，最后通过这组样本的加权求和作为状态的估计值。粒子滤波不受系统模型的线性和高斯假设约束，采用样本形式而不是函数形式对状态概率密度进行描述，使其不需要对状态变量的概率分布作过多的约束，适用于任意非线性非高斯动态系统，是目前最适合于非线性、非高斯系统状态的滤波方法。
在上世纪50年代，有了序贯重要性采样(Sequential Importance Sampling，SIS)算法，一种通过离散随机样本逼近概率分布的蒙特卡罗方法（Monte Carlo）。后来针对SIS算法中的粒子退化(degeneracy)问题，1993年Gardon等人重采样(resampling)，得到新的算法–序列重要性重采样算法（Sequential Importance Resampling，SIR）。但是还没完，SIR算法又出现粒子多样性匮乏问题，针对退化和多样性匮乏这两个问题，又提出下面两个解决办法：
- 优化重采样，减小粒子多样性匮乏：目前最常用最简单的四种重采样算法：(1)多项式重采样算法,(2)残差重采样算法,(3)分层重采样算法,(4)系统重采样算法，这四种方法以速度和结果综合考量，系统重采样最优。当然又更好的算法：Gibbs采样器和Metropolis-Hasting方法等马尔可夫链蒙特卡洛(MCMC)方法解决粒子退化问题。针对重采样，科学家们搞出来了遗传粒子滤波器，正则化粒子滤波（regularized particle filter，RPF）算法，辅助粒子滤波（auxiliary particle filter,APF）算法。
- 产生既接近系统状态真实概率密度又便于从中抽样的重要性概率密度：如UPF(Unscented Particle Filter),高斯粒子滤波（Gaussian particle filter,GPF）算法,高斯混合粒子滤波（Gaussian sum particle filter,GSP）算法.
名词解释
- 1.粒子退化(degeneracy)问题: 样本（又称为粒子）退化是指经过若干次迭代后，大部分粒子的权值极小以至于可以忽略不计，粒子权值的方差随时间逐渐增大，大量运算时间浪费在这些对系统状态估计几乎不起作用的粒子权值更新上。
- 2.粒子多样性匮乏问题：SIR算法的重采样在一定程度上缓解了粒子退化问题，但是它的重采样在原有粒子集合组成的离散分布上依粒子权值大小，采取随机选取复制的方式进行，即采用以较大概率复制权值较高粒子的方法。这样就使原有粒子集合中有很多粒子由于权值太小没有“后代”，而少数权值较大的粒子则有很多相同的“后代”，导致重采样得到的粒子集合由大量重复粒子构成，使粒子集合失去了多样性，从而限制了粒子并行运行的机会。这一现象又称为粒子多样性匮乏问题。
- 3.重要性密度函数：
PF的收敛性分析
在粒子滤波算法收敛性分析上，标准粒子滤波满足中心极限定理。标准粒子滤波产生的状态近似分布几乎肯定收敛（弱收敛）于状态后验分布。为了找到一个合适的后验概率密度函数，我们就可以通过采样(多用蒙特卡罗方法采样)来去近似这个概率密度函数(Probability Density Function,PDF)(不知道近似这个词用的准不准确)，因为他满足中心极限定理，所以我们才可以用采样的方法，样本数量越多，自然越好。
上面简单的介绍了一下PF的背景，下面开始进入正题，我也是最近因为课设接触了粒子滤波，大部分理论是直接copy别人的论文上面的，参考文献附在文章最后，我觉得对于一个不了解刚入门的东西，看CNKI的文献比直接阅读英文文献要好一点(尤其是那些博士论文的背景介绍部分，讲的很详细的，可以当科普文章看)，毕竟前辈们用更加严谨的说法用我的母语来讲述PF的理论，比我自己说的要好的多，也更能轻松的让我理解，copy过来也是为自己以后复习方便，中间加入自己的一些理解，如果有大神看到错误，恳请指出~
Method
估计问题一般分为三类：
- (1)从当前和过去的观测值来估计信号的当前值，称为滤波；
- (2)从过去的观测值来估计信号的现在值或将来值，称为预测或外推；
- (3)从过去的观测值来估计过去的信号值，称为平滑或内插。
动态系统的状态空间模型
动态系统是指输出信号不仅取决于同时刻的激励信号，而且与它过去的工作状态有关的系统。
状态空间模型是描述动态系统的完整模型，它用系统方程来描述状态随时间演变的过程，并用观测方程来描述与状态有关的噪声变量，表达了由于输入引起系统内部状态的变化并由此使输出发生的变化。
状态空间模型包括：（1）系统模型，用于描述系统状态随时间演变的过程；（2）观测模型，用于将系统在某时刻的输出和系统的状态联系起来。
\begin{equation} x_k = f(x_{k-1}, {\mu}_{k-1}) \tag{1} \end{equation}\begin{equation}
y_k = h(x_k, v_k) \tag{2}
\end{equation}
其中，$f(.)$和$h(.)$为已知函数，系统噪声$x_k$和观测噪声$v_k$是概率密度已知的随机变量，$x_k$代表系统在$k$时刻的状态变量，$y_k$代表$x_k$的观测值，$u_k$和$v_k$相互独立且独立于系统状态.
这里(1)就是系统方程，(2)就是观测方程，在实际问题中，$y_k$可能就是一组由传感器得到的数据，意思就是说$y_k$不一定是由观测方程算出来的，而是直接得到的，比如室内的温度，可以直接由温度计得到，这就是观测数据，系统方程提供的是预测值，我们可以根据观测值来修正预测值，在MATLAB官网上file exchange中提供的particle filter tutorial中，则是利用观测方程来生成观测数据，因为这是仿真，所以用观测方程来模拟生成，而不是用传感器直接得到。
上述状态空间模型的统计描述方式如下：
- (1)对应于系统模型有系统的状态转移概率密度 \begin{equation} p(x_k|x_{k-1}) \end{equation}
- (2)对应于观测模型有系统状态的观测似然概率密度 \begin{equation} p(y_k|x_k) \end{equation}
并假设：系统状态$x_k$服从一阶马尔可夫过程，系统观测$y_k$独立，已知系统状态的初始先验密度(Prior Density)为$p(x_0)$.
P.S. 一阶markov过程在这里的意思就是说系统状态$x_k$只和$x_{k-1}$有关，和$k-1$之前的状态(例如$x_{k-2}$)通通无关。同理如果$x_k$和$x_{k-1}$，$x_{k-2}$有关，和$k-2$之前的状态(例如$x_{k-3}$)通通无关就叫二阶markov过程。依次类推。Bayes 滤波理论
引言
贝叶斯估计理论及其方法为动态系统的估计问题提供了一类严谨的解决框架，它利用已知的信息建立系统状态的概率密度估计，可以得到对系统状态估计的最优解。贝叶斯估计利用系统方程预测状态的先验概率密度，再利用最新的观测值进行修正，得到状态的后验概率密度，包括了观测值和先验知识在内的所有可利用的信息。
卡尔曼滤波、扩展卡尔曼滤波和粒子滤波都是基于状态空间模型的贝叶斯估计具体实现算法。
卡尔曼滤波要求系统线性和噪声服从高斯分布；扩展卡尔曼滤波同样要求噪声服从高斯分布。而粒子滤波不用满足系统为线性、噪声高斯分布的限制条件。粒子滤波是蒙特卡罗方法和贝叶斯估计理论结合的产物，它通过非参数化的蒙特卡罗模拟方法从时域实现递推贝叶斯估计。
贝叶斯估计的主要目的是利用先验知识和实际观测数据来构造未知系统状态的概率密度函数。 设$k$代表时间，$x_{0:k}$代表未知变量列即$x_{0:k}=\{{x_i:i=0,...,k}\}$,$y_{1:k}$代表观测变量列即$y_{1:k}=\{y_i:i=1,...,k\}$,若给定观测量$y_{1:k}$，则未知量$x_{0:k}$的条件概率分布为: \begin{equation} p(x_{0:k}|y_{1:k}) = \frac{p(x_{0:k})p(y_{1:k}|x_{0:k})}{\int p(y_{1:k}|x_{0:k})p(x_{0:k})dx_{0:k}} \tag{3} \end{equation} 其中，$p(x_{0:k})$称为先验概率密度，$p(y_{1:k}|x_{0:k})$称为观测量为$y_{1:k}$时的似然概率密度，$p(x_{0:k}|y_{1:k})$称为后验概率密度，这就是贝叶斯定理。
从上式可见，通过似然概率密度观测数据修正了先验信息。贝叶斯定理描述了从先验知识（先验概率密度）开始，不断利用陆续到来的新观测数据修正先验知识，从而得到修正后的未知量知识（后验概率密度）的过程。先验概率密度归纳了新观测数据到达之前未知状态变量的所有信息。似然概率密度表达了在实际观测数据已知的前提下未知状态变量出现的概率，是实际观测数据和未知变量之间的逻辑联系。由于得到新观测数据的修正，后验概率密度较之先验概率密度更加接近被估计量的真实概率密度。而这个后验概率密度又是下一个新观测量到来时被估计量的先验概率密度。 上述后验概率密度$p(x_{0:k}|y_{1:k})$是贝叶斯估计问题的完整解。 而滤波问题就是要计算后验滤波概率密度$p(x_k|y_{1:k})$,它是$p(x_{0:k}|y_{1:k})$的边沿密度即 \begin{equation} p(x_{0:k}|y_{1:k}) = \int \int .. \int p(x_{0:k}|y_{1:k})dx_0dx_1...d_{x-1} \tag{4} \end{equation} 对于动态系统，从该后验滤波概率密度就可以计算系统状态的各种估计，如以均值作为系统状态的估值等.由(3)和(4)式可见，当每一个新观测数据到来时，后验滤波概率密度$p(x_k|y_{1:k})$就要被重新计算一次，这是十分不方便的。为此，采用如下递推更新方法来获得该后验滤波概率密度:- (1)预测—从$k-1$时刻所得到的后验滤波概率密度$p(x_{k-1}|y_{1:k-1})$出发，利用系统模型来预测$k$时刻$x_k$的概率密度，得到$k$时刻$x_k$的先验滤波概率密度$p(x_k|y_{1:k-1})$;
- (2)更新—当$k$时刻的观测值$y_k$到来时，利用它修正上述先验滤波概率密度，从而得到$k$时刻的后验滤波概率密度$p(x_k|y_{1:k})$。
设$k-1$时刻滤波概率密度$p(x_{k-1}|y_{1:k-1})$已知，假设系统状态$x_k$服从一阶马尔可夫过程且系统观测$y_k$独立。
递推更新求后验PDF
首先，通过预测步骤得到不包含$k$时刻观测值的$k$时刻系统状态先验滤波概率密度$p(x_k|y_{1:k-1})$如下:
\begin{equation}
p(x_k|y_{1:k-1}) = \int p(x_k|x_{k-1}) p(x_{k-1}|y_{1:k-1}) dx_{k-1} \tag{5}
\end{equation}
其中，$p(x_k|x_{k-1})$是系统状态的转移概率密度。
文献[2]中指出式(5)即Chapman–Kolmogorov equation,然而我并没有看懂wiki里的解释。下面解释一下式(5)是怎么得到的：
- (1)首先我们知道$x_k$服从一阶markov过程，所以$p(x_k|x_{k-1}, y_{1:k-1}) = p(x_k|x_{k-1})$ ;
- (2)把上式带入式(5)中，利用Bayes公式替换即可，具体如下： \begin{equation} p(x_k|y_{1:k-1}) = \int p(x_k|x_{k-1}) p(x_{k-1}|y_{1:k-1}) dx_{k-1} \quad=\int p(x_k|x_{k-1}, y_{1:k-1}) p(x_{k-1}|y_{1:k-1}) dx_{k-1} \\ \quad=\int \frac{p(x_k,x_{k-1}, y_{1:k-1})}{p(x_{k-1}, y_{1:k-1})}\frac{p(x_{k-1},y_{1:k-1})}{p(y_{1:k-1})}dx_{k-1} \quad=\int \frac{p(x_k,x_{k-1}, y_{1:k-1})}{p(y_{1:k-1})} dx_{k-1} \quad=\int p(x_k,x_{k-1})| p(y_{1:k-1}) dx_{k-1} \tag{6} \end{equation}
然后，利用$k$时刻的观测值$y_k$，通过更新步骤修正$p(x_k|y_{1:k-1})$，得到$k$时刻系统状态后验滤波概率密度$p(x_k|y_{1:k})$，其推导过程如下：
由贝叶斯公式有,
$$p(x_k|y_{1:k}) = \frac{p(x_k)p(y_{1:k}|x_k)}{p(y_{1:k})} = \frac{p(x_k)p(y_k, y_{1:k-1}|x_k)}{p(y_k, y_{1:k-1})} \tag{7}$$
由条件概率定义有:
$$p(y_k, y_{1:k-1}) = p(y_{1:k-1}) p(y_k | y_{1:k-1}) \tag{8}$$
由联合分布概率公式有:
$$p(y_k, y_{1:k-1}|x_k) = \frac{p(y_k, y_{1:k-1}, x_k)}{p(x_k)}
= \frac{p(y_k, y_{1:k-1}, x_k)}{p(y_{1:k-1}, x_k)} \frac{p(y_{1:k-1}, x_k)}{p(x_k)} = p(y_k|y_{1:k-1}, x_k)p(y_{1:k-1}|x_k) \tag{9}$$
又由贝叶斯公式有:
$$p(y_{1:k-1}|x_k) = \frac{p(y_{1:k-1})p(x_k|y_{1:k-1})}{p(x_k)} \tag{10}$$
将式(8)(9)(10)代入式(7)得:
$$p(x_k|y_{1:k}) = \frac{p(y_k| y_{1:k-1}, x_k) p(x_k|y_{1:k-1}) p(y_{1:k-1}) p(x_k)}{p(x_k) p(y_{1:k-1}) p(y_k|y_{1:k-1})} \tag{11}$$
根据假设各个系统观测$y_k$相互独立，即有:
$$p(y_k|y_{1:k-1}, x_k) = p(y_k|x_k) \tag{12}$$
将(12)式代入(11)式并消去分子和分母的共同项,得后验滤波概率密度 $p(x_k|y_{1:k})$如下:
$$p(x_k|y_{1:k}) = \frac{p(y_k|x_k) p(x_k|y_{1:k-1})}{p(y_k|y_{1:k-1})} \tag{13}$$
其中，$p(y_k|x_k)$是似然概率密度，它与观测方程和观测噪声$v_k$的统计特性有关：
$$p(y_k|x_k) = \int \delta (y_k - h(x_k, v_k))p(v_k) dv_k \tag{14}$$
这里，$\delta(.)$是Dirac delta函数，$p(v_k)$是$v_k$的概率密度。然而我并不知道式(14)是怎么得来的，是什么道理。而
$$p(y_k|y_{1:k-1}) = \int p(v_k|x_k) p(x_k|y_{1:k-1}) dx_k \tag{15}$$
它一般是一个归一化常数。
这样由(5)式和(13)式就构成了后验滤波概率密度的递推公式，实现了由$k-1$时刻后验滤波概率密度$p(x_{k-1}|y_{1:k-1})$到$k$时刻后验滤波概率密度$p(x_k|y_{1:k})$的递推更新过程，从理论上提供了求后验滤波概率密度的方法。
但实际问题中只有很少类型的系统可以直接利用上述解析方法求得后验滤波概率密度。例如，针对线性高斯系统的卡尔曼滤波器。对于大多数动态系统，由于各种原因（例如，在很多实际问题中上述(5)式的积分是很难实现的），直接利用上述方法很难求得后验滤波概率密度的解析解，该方法在实际操作中受到较大的限制。下面的贝叶斯重要性采样和序贯重要性采样给出了这个问题的解决方法。
重要性采样
蒙特卡罗方法
蒙特卡罗(Monte Carlo, MC)方法,又称随机抽样或统计模拟方法，蒙特卡罗算法并不是一种算法的名称，而是对一类随机算法的特性的概括。此外，还有一种随机算法叫拉斯维加斯算法(Las Vegas algorithm)。
- 蒙特卡罗算法：采样越多，越近似最优解；
- 拉斯维加斯算法：采样越多，越有机会找到最优解。
怎么理解上面这两句话，可以参考这里,讲的深入浅出，简单易懂。按照wikipedia里的解释,MC Method 万变不离其宗，可以概括为以下内容(为了保证理解，我就不用我三脚猫的功夫来翻译了)：
- (1) Define a domain of possible inputs；
- (2) Generate inputs randomly from a probability distribution over the domain;
- (3) Perform a deterministic computation on the inputs;
- (4) Aggregate the results.
举个例子：我们知道圆和它的的外接正方形的面积比是$\pi/4$,然后我们根据MC Method来估计$\pi$的取值，如下：
- (1) 首先画个圆，然后在其基础上画出它的外接正方形；
- (2) 然后我们往里面撒米啊或者沙子之类的东西，注意不能随便撒，得保持均匀的撒，英文用的词叫uniform；
- (3) 然后统计圆里面的米粒个数和整个正方形里的个数；
- (4) 最后我们计算出米粒的比值，这个比值我们就可以认为接近$\pi/4$，然后在乘以4，就能得到$\pi$的估值。
在上面这个例子中，正方形就是我们之前提到的domain of possible inputs，我们撒米粒就相当于Generate random inputs，注意我们需要按照a probability distribution来撒。最后我们得到统计来得到近似值。
有两点需要注意：
- (1)如果米粒不是uniformly distributed，那么我们最后得到的approximation，即$\pi$的估计值就会很离谱，或者说不接近真实值，英文说是poor，我不知道如何翻译比较恰当。
- (2)我们必须有大量的input，即我们要撒很多米粒，样本足够多式，我们才能认为结果不是poor的，可以参考大数定理。这样侧面印证上面说的，采样越多，越近似最优解。
贝叶斯重要性采样
通常，求解后验概率密度并不是我们的最终目的，我们希望通过这个后验概率密度得到服从这个概率密度分布的某随机变量的某函数的估计值，如期望值或方差等，而这个后验概率密度只是一个用来计算这些值的工具.例如，设 $g(x_{0:k})$为状态变量$x_{0:k}$的任意函数，则$g(x_{0:k})$ 的数学期望为：
$$E(g(x_{0:k})) = \int g(x_{0:k}) p(x_{0:k} | y_{1:k}) dx_{0:k} \tag{16}$$
根据蒙特卡罗仿真原理,可以从后验概率密度 $p(x_{0:k} | y_{1:k})$ 中抽取$N$个个独立同分布(i.i.d.)的样本$\{x_{0:k}(i), i=1,2,…,N\}$，使
$$E(g(x_{0:k})) \approx \frac{1}{N} \sum_{i=1}^{N} g(x_{0:k}(i)) \tag{17}$$
当$N$足够大时，该近似值绝对收敛于 $E(g(x_{0:k}))$ .
实际问题中，通常较难甚至无法直接从后验概率密度 $p(x_{0:k} | y_{1:k})$中抽样。于是，引入一个被称为重要性概率密度的参考分布$q(x_{0:k} | y_{1:k})$ ,该参考分布应已知且便于从中抽样。利用该重要性概率密度和贝叶斯公式有:
$$E(g(x_{0:k})) = \int g(x_{0:k}) \frac{p(x_{0:k}|y_{1:k})}{q(x_{0:k} | y_{1:k})} q(x_{0:k} | y_{1:k}) dx_{0:k} = \int g(x_{0:k}) \frac{p(y_{1:k}| x_{0:k}) p(x_{0:k})}{p(y_{1:k}) q(x_{0:k} | y_{1:k})} q(x_{0:k} | y_{1:k}) dx_{0:k} \\ = \int g(x_{0:k}) \frac{{w_k}^{*}(x_{0:k})}{p(y_{1:k})} q(x_{0:k} | y_{1:k}) dx_{0:k} \tag{18}$$ 上式令权值 ${w_k}^{*}(x_{0:k})$ 为 $${w_k}^{*}(x_{0:k}) = \frac{p(y_{1:k} | x_{0:k}) p(x_{0:k})}{q(x_{0:k} | y_{1:k})} \tag{19} $$ 因为(18)式是对$x_{0:k}$积分，故$y_{1:k}$可看作常数处理，从而 $$ E(g(x_{0:k})) = \frac{\int g(x_{0:k}) {w_k}^{*}(x_{0:k}) q(x_{0:k}|y_{1:k})}{p(y_{1:k})} \tag{20}$$ 又因为 $$ p(y_{1:k}) = \int p(y_{1:k}, x_{0:k}) dx_{0:k} = \int \frac{p(y_{1:k}|x_{0:k}) p(x_{0:k}) q(x_{0:k}|y_{1:k})}{q(x_{0:k}|y_{1:k})} dx_{0:k} = \int {w_k}^{*}(x_{0:k}) q(x_{0:k}|y_{1:k}) dx_{0:k} \tag{21} $$ 将(21)代入(20)得： $$ E(g(x_{0:k})) = \frac {\int g(x_{0:k}) {w_k}^{*}(x_{0:k}) q(x_{0:k}|y_{1:k}) dx_{0:k}} {\int {w_k}^{*}(x_{0:k}) q(x_{0:k}|y_{1:k}) dx_{0:k}} \tag{22} $$依据蒙特卡罗仿真原理，从重要性概率密度 $q(x_{0:k}|y_{1:k})$中抽取$N$个独立同分布的样本$\{x_{0:k}(i), i=1,2,…,N \}$ ,则上述数学期望可近似表示为：
$$E(g(x_{0:k})) \approx \frac{\frac{1}{N} \sum_{i=1}^{N} g(x_{0:k}(i)) {w_k}^{*}(x_{0:k}(i)) }{\frac{1}{N} \sum_{i=1}^{N} {w_k}^{*}(x_{0:k}(i)) } = \sum_{i=1}^{N} g(x_{0:k}(i)) {w_k}(x_{0:k}(i)) \tag{23}$$其中，
$$ w_k(x_{0:k}(i)) = \frac{ {w_k}^{*}(x_{0:k}(i))}{ \sum_{i=1}^{N} {w_k}^{*}(x_{0:k}(i))} \tag{24}$$
称为归一化权值。
贝叶斯重要性采样的核心思想是利用一系列随机样本及其权值来近似表示所需的后验概率密度，进而得到所需统计量的估计值。一个随机变量的后验概率密度可以用一系列离散样本及其权值来近似表示，近似的程度高低依赖于样本的数量$N$。通常，随机变量的后验概率密度无法直接得到的，而贝叶斯重要性采样提供了一种解决该问题的方法。
序贯重要性采样
由上述分析可见，贝叶斯重要性采样方法存在如下缺陷：每当新的观测数据 $y_k$到来时都需要重新从重要性概率密度 $q(x_{0:k} | y_{1:k})$中抽取样本$\{x_{0:k}(i),i=1,...,N\}$,并且需要按照式(19)重新计算每个$x_{0:k}(i)$ 的权值$w_k(x_{0:k}(i))$ ，这就会占用很大的存储空间以及增大了计算量，不便于实际应用。序贯重要性采样（Sequential Importance Sampling,SIS）将贝叶斯重要性采样方法写成序列形式，其权值采用递归更新方式计算，使概率密度估计以递推方式实现。
为了递推估计概率密度，重要性概率密度 $q(x_{0:k} | 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}) \tag{25}$$
即随着观测数据的不断到来，新样本 $x_{0:k-1}(i) \sim q(x_{0:k}|y_{1:k})$可以通过将新数据$x_k(i) \sim q(x_k|x_{0:k-1}(i), y_{1:k})$添加进旧样本$x_{0:k-1}(i) \sim q(x_{0:k-1} | y_{1:k-1})$ 中得到。
将(25)式代入(19)式得：
$$ {w}^{*}_k (x_{0:k}) = \frac{p(y_{1:k} | x_{0:k}) p(x_{0:k})}{q(x_k | x_{0:k-1}, y_{1:k}) q(x_{0:k-1} | y_{1:k-1}) } \tag{26} $$
又由(19)式还可得：
$$ {w}^{*}_{k-1} (x_{0:k-1}) = \frac{p(y_{1:k} | x_{0:k}) p(x_{0:k})}{ q(x_{0:k-1} | y_{1:k-1}) } \tag{27} $$
这样由(26)(27)式可得:
$$ {w}^{*}_{k}(x_{0:k}) = {w}^{*}_{k-1} (x_{0:k-1}) \frac{p(y_k | x_k) p(x_k | x_{k-1})}{ q(x_k | x_{0:k-1}, y_k) } \tag{27} $$
SIS算法
SIS算法实现的步骤如下：
- (1)预测: $x_k(i) \sim q(x_k|x_{0:k-1}(i), y_{1:k}) (i = 1,2,...,N) $ 得到新样本 $x_{0:k}(i) = \{x_k(i), x_{0:k-1}(i)\}$ ;
- (2)更新: 利用式(28)计算各新样本的权值 ${w}^{*}_k (x_{0:k}(i))$ 如下：
$$ {w}^{*}_k(x_{0:k}(i)) = {w}^{*}_{k-1}(x_{0: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_{1:k})} \tag{28} $$
归一化上述权值如下:
$$ w_k(x_{0:k}(i)) = \frac{ {w_k}^{*}(x_{0:k}(i))}{ q(x_k(i) | x_{0:k-1}(i), y_{1:k}) } \tag{29} $$
从而得到一组加权样本$\{ (x_{0:k}(i), w_k(x_{0:k}(i))), i=1,2,...,N\}$ ,则所求概率密度可用这组样本的加权和近似表示为:
$$p(x_{0:k} | y_{1:k}) \approx \sum_{i=1}^{N} w_k(x_{0:k})\delta(x_{0:k} - x_{0:k}(i)) \tag{30}$$ 下文中我们一律将$ {w}^{*}_k(x_{0:k}(i))$ 简写为 ${w}^{*}_k(i)$, 将$ w_k(x_{0:k}(i))$简写为$w_k(i)$ 。
结合(4)式和(31)式可知，$x_k$的滤波概率密度可近似表示为
$$p(x_k|y_{1:k}) \approx \sum_{i=1}^{N} w_k(i)\delta(x_k-x_k(i)) \tag{31}$$ $x_k$的最小均方误差估计值(minimum mean squareerror estimation, MMSE)为： $$ \hat{x_k} = \sum_{i=1}^{N} w_k(i) x_k(i) \tag{32}$$ 序贯重要性采样的样本权值采用便于计算的递归更新方式，并且其强大数定理和中心极限定理成立。序贯重要性采样算法是递推的贝叶斯重要性采样算法，是粒子滤波的基础。到目前为止，各种不同的粒子滤波算法都是基于序贯重要性采样算法的。
SIR算法
序贯重要性采样算法中存在的基本问题是样本退化问题。它指算法经过若干次迭代后，样本权值的方差会随时间逐渐增大，使得少数样本的权值很大，而大多数样本的权值很小，以至于可以忽略不计。这意味着大量计算工作都浪费在对求解滤波概率密度几乎不起任何作用的样本的权值更新上。消除样本退化的主要手段有两种：选取适当的重要性概率密度和样本重采样。一种简便易行且使用较为广泛的方法就是选择具有先验性质的系统状态转移概率密度作为重要性概率密度，即
$$ q(x_k | x_{0:k-1}(i), y_{1:k}) = p(x_k|x_{k-1}(i)) \tag{33}$$
将式(33)代入式(29)得：
$$ {w}^{*}_k(i) = {w}^{*}_{k-1}(i) p(y_k|x_k(i)) \tag{34}$$
但该方法的缺点是没有利用最新观测信息$y_k$.从状态转移概率密度中采样所得样本的权值方差较大，未能克服样本权值的退化问题。但由于其形式更简单且易于实现，在各种粒子滤波算法中己经被广泛使用。
减小退化的另一种方法是采用重采样技术。重采样的基本思想是对(31)式或(32)式中后验概率密度的离散近似表示再进行一次采样，繁殖权值较高的样本而淘汰权值较低的样本，重新生成一个新样本集合，以克服样本权值退化问题。由于新样本独立同分布，因此重采样所得新样本的权值均为$1/N$.
1993 年 Gordon 等为了克服 SIS 算法中的样本退化问题，首次将重采样(resampling)步骤引入 SIS 算法，并由此产生了基本粒子滤波算法—序列重要性重采样算法（sequential importance resampling，SIR）。在粒子滤波中样本又被称为粒子，样本退化又称为粒子退化。SIR 算法选择先验的系统状态转移概率密度作为重要性概率密度，在 SIS 算法的基础上增加了重采样步骤，由预测、更新和重采样三部分组成。SIR 算法中的重采样采用的是多项式重采样算法，其过程如下：
每次从$[0,1]$上的均匀分布中随机抽取一个样本$\mu \sim U[0,1]$, 满足下式的粒子$x_k(i)$被选出并复制到新的粒子集合中
$$ \sum_{j=1}^{i-1}w_k(j) \le \mu \le \sum_{j=1}^{i}w_k(j) \tag{35}$$
该过程重复$N$次，得到$N$个新粒子，组成一个新粒子集合，每个新粒子的权值均为 $1/N$。上述过程等价于从参数为$(N, w_k(i))$的多项式分布中抽样。此后，Liu、Kitagawa 和 Doucet 等人又在多项式重采样算法的基础上分别提出了残差重采样(Residual Resampling) 算法、分层重采样(Stratified Resampling)算法和系统重采样(Systematic Resampling)算法。从滤波精度和计算量方面综合考虑，上述四种重采样算法中以系统重采样算法较优。
SIR 算法的步骤如下：
- (1)预测:从系统状态转移概率密度中抽取新粒子: $x_k(i) \sim p(x_k|x_{k-1}(i)) (i=1,2,...,N)$
- (2)更新:利用式(34)计算各新样本的权值 ${w_k}^{*}(i)$ 并归一化如下： $$ w_k(i) = \frac{ {w_k}^{*}}{ \sum_{i=1}^{N} {w_k}^{*}(i)} \tag{36}$$
- (3)状态估计: $$ \hat{x_k} = \sum_{i=1}^{N} {w_k}(i)x_k(i) \tag{37}$$
- (4)进行多项式重采样。
Resample算法
引言
重采样是解决粒子退化问题的一种重要技术手段，自粒子滤波出现以来众多国内外学者一直致力于重采样技术的研究。1993年Gordon等人为了克服粒子退化问题，将多项式重采样(Multinomial Resampling)算法应用在他们提出的SIR粒子滤波算法中。多项式重采样在一定程度上缓解了粒子退化问题，但是它的计算量较大。1993年Liu等人在多项式重采样方法的基础上提出了残差重采样(Residual Resampling) 算法，其计算量大大小于多项式重采样算法。1996年Kitagawa等人对多项式重采样算法进行了改进，进而提出了分层重采样(Stratified Resampling)算法。
同年M.Pitt和N.Shephard在他们提出的辅助粒子滤波（APF）算法中，利用下一个时刻系统状态的观测值来指导上一个时刻的粒子重采样。由于该重采样方法考虑了系统状态的最新观测值，其在系统噪声较小时获得了较好的估计性能改进。
2000年Doucet等人在分层重采样算法的基础上提出了系统重采样(Systematic Resampling)算法。上述重采样算法均采用在离散分布 $\{ {x_k(i), w_k(i)}_{i=1}^{N}\}$ 上以较大概率复制权值较高粒子的方法进行重采样，其缺陷是存在粒子多样性匮乏问题。
针对粒子多样性匮乏问题，2001年Musso等提出正则化（regularized）重采样算法。该算法在一个连续分布上进行重采样，改善了重采样带来的粒子多样性匮乏问题，在系统噪声较小时改善了估计性能。但该算法有一个理论上的缺陷，即重采样后所得的粒子群不能保证渐进地接近所求分布。此外，还出现了一类在重采样过程中增加马尔可夫蒙特卡罗(Markov Chain Monte Carlo, MCMC)移动步骤的重采样算法。
粒子退化的程度可以采用有效样本数（effective sample size） $N_{eff}$ 来衡量，其定义如下:
$$ n_{eff} = \frac{N}{1+Var({w_k}^{'}(i))} \tag{38}$$
其中，$N$为实际使用的粒子数，$Var(.)$代表方差函数，${w_k}^{'}$被称为“真实权值”,其定义如下：
$${w_k}^{'}(i) = \frac{p(x_k(i) | y_{1:k})}{q(x_k(i) | x_{k-1}(i), y_k)} \tag{39}$$
由(38)式可见，等号右端分式的分子部分实际上就是粒子滤波最终所要逼近的后验概率密度函数，因此上述有效样本数在计算上存在较大困难。实际应用中通常采用下式来得到 $N_{eff}$ 的估计值 $\hat{N_{eff}}$ :
$$ \hat{ N_{eff}} = \frac{1}{ \sum_{i=1}^{N} {(w_k(i))}^2} \tag{40}$$
其中，$w_k(i)$是由(29)式计算得到并归一化后的权值。
有效样本数的值越小表示粒子退化程度越严重。由(38)式可见，增大实际使用的粒子数量可以减小粒子退化程度，但过大的粒子数量会带来计算量增大的问题，尤其当被估计变量的维数较高时。
对于何时进行重采样，一种观点是每次迭代都进行重采样；另一种观点是不必每次都重采样，只是当有效样本数小于某个阈值，即 $N_{eff} < N_{th}$ 时才重采样,，这称为自适应重采样。自适应重采样的主要优点是计算量较小，但如何确定恰当的阈值要依据具体的问题而定，目前还没有通用的解决方案。
基本重采样算法
自从粒子滤波出现以来，为了克服粒子退化现象出现了大量的粒子重采样技术。其中，应用最为广泛的一类采用在离散分布 ${ \{ x_k(i), w_k(i)\} }^{N}_{i=1}$ 上以较大概率复制权值较高粒子的方法进行，该类重采样方法一般满足如下约束:
$$ \sum_{i=1}^{N} N_i = N \tag{41}$$
$$\hat{w_k(i)} = \frac{1}{N} \tag{42}$$
$$E(N_i) = N w_k(i) \tag{43}$$
其中，$N_i$代表第$i$个粒子经过重采样后被复制的次数，$w_k(i)$代表重采样前一归一化的粒子权值，$\hat{w_k(i)}$代表重采样得到的第$i$个粒子的权值.
这类重采样通过去除低权值粒子、繁殖(复制)高权值粒子的方式来修正滤波中得到的加权近似概率密度，得到一个等权近似概率密度。。该类重采样方法的典型代表包括：多项式重采样算法、残差重采样算法、分层重采样算法和系统重采样算法等。
多项式重采样
多项式重采样算法首先利用重采样之前的粒子权值集合${ \{ w_k(i) \} }^{N}_{i=1}$组成一个多项式分布即 $Mult(N;w_k(1),w_k(2),...,w_k(N))$,并从该多项式分布中抽样$N$次得到$N$个序号（其中每个序号取值为$0 \sim N$之间的整数且$N$个序号之和等于$N$）; 然后，令$N_i$表示序号值等于$i$的序号的总个数，分别统计各序号值相等的序号的总个数从而得到${ \{ N_i \}}^N_{i=1}$；最后，将${ \{ x_k(i) \} }^N_{i=1}$中各$x_k(i)$复制$N_i$次到一个新粒子集合中，该新粒子集合就是重采样后得到的粒子集合，并且新粒子集合中每个粒子的权值等于$1/N$。该算法的执行步骤如下所示:(1)从多项式 $Mult(N;w_k(1),w_k(2),...,w_k(N))$中抽样得到 ${ \{N_i\} }^N_{i=1}$ :
- 从区间为$(0,1]$的均匀分布上抽样得到$N$个独立同分布样本点 ${ \{ u(i)\} }^N_{i=1} $ ;
- 定义序号函数$D(.)$如下(其中$i,m = 1,2,…,N$) $$D(u(i)) = m, 如果u(i) \in \left( \sum_{j=1}^{m-1} w_k(j), \sum_{j=1}^{m} w_k(j) \right] \tag{43}$$ 将集合 ${\{ u(i)\}}^N_{i=1}$ 中各 $u(i)$代入(43)式计算得到序号集合 ${\{ D(u(i))\}}^N_{i=1}$, 统计各序号值相等的序号的总个数,从而得到 ${\{N_i \}}^N_{i=1}$;
(2)将各$x_k(i)$复制$N_i$次到新粒子集合中组成重采样后的粒子集合，并且给每个粒子的权值赋值为$1/N$.
残差重采样算法
由于多项式重采样算法需要在上述多项式分布上抽样$N$次，因此该算法计算量较大。残差重采样算法在多项式重采样算法基础上进行了改进，降低了算法的计算量，并且重采样后粒子的均值方差小于多项式重采样算法。残差重采样算法的执行步骤如下所示:
(1) 从多项式分布 $Mult(N-R; w_k^{'}(1),w_k^{'}(2),…,w^{'}_k (N))$中抽样得到 ${ \{N_i^{'} \} }^N_{i=1}$ ，其中
$$R = \sum_{i=1}^{N} \lfloor Nw_k(i) \rfloor \tag{44} $$ $$w_k^{'}(i) = \frac{Nw_k(i) - \lfloor Nw_k(i) \rfloor }{N-R} \tag{45} $$ 这里，$i=1,2,…,N;\lfloor \rfloor $表示取$Nw_k(i)$的整数部分.令$N_i = \lfloor Nw_k(i) \rfloor + N_k^{'}$,得到 ${\{N_i \}}^{N}_{i=1}$;(2) 将各$x_k (i)$复制$N_i$次到新粒子集合中组成重采样的粒子集合，并且给每个粒子的权值赋值为$1/N$。
由上述步骤(1)可见残差重采样算法值需在多项式分布上抽样$N-R$次。与多项式重采样算法相比，其计算量大大降低。
分层重采样算法
分层重采样算法也是基于多项式重采样算法的改进算法，它所得到的粒子的均值方差也小于多项式重采样算法。该算法将区间$(0,1]$划分为$N$个连续且不重合的子区间，在每个子区间上按均匀分布进行一次抽样，共得到$N$个样本点${ {u(i) }}^N_{i=1}$。分层重采样算法执行步骤如下所示：
- (1)将区间$(0,1]$划分为$N$连续且不重合的子区间即 $$ (0,1] = (0,\frac{1}{N}) \bigcup (\frac{1}{N}, \frac{2}{N}] \bigcup ... \bigcup (\frac{N-1}{N}, 1] \tag{46}$$ 在上述每个子区间上按均匀分布各进行一次抽样，共得到$N$个样本点${\{ u(i)\}}^N_{i=1}$;再同多项式重采样算法一样，利用(43)式得到${\{ u(i)\}}^N_{i=1}$
- (2)同多项式重采样算法的第（2）步。
系统重采样方法
在分层重采样算法中，抽样得到的$N$个样本点${\{u(i)\}}^N_{i=1}$之间存在一定的确定性关系（其排列顺序从小到大）。系统重采样算法是分层重采样算法的改进算法，它得到的$N$个样本点${\{u(i)\}}^N_{i=1}$之间存在着更强的确定性关系，这使其计算量小于分层重采样算法。系统重采样算法的执行步骤如下所示:(1)在区间$(0, 1/N)$的均匀分布抽取一个样本$U$，按下式可计算得到 ${\{u(i)\}}^N_{i=1}$ $$u(i) = \frac{i-1}{N} + U , i = 1,2,...,N \tag{46}$$ 再同多项式重采样算法一样，利用(43)得到 ${\{ N_i\}}^N_{i=1}$
(2)同多项式重采样算法的第(2)步。
由于抽样得到的样本点之间存在一定的确定性关系，实际应用中分层重采样算法和系统重采样算法对粒子序号的排序较为敏感，重采样之前粒子序号的不同排序同常会导致重采样之后得到不同分布的粒子集合。而多项式重采样算法和残差重采样算法则对粒子序号的排序不敏感.
Result&Discussion
以上只是基本的粒子滤波算法，回顾一下整个流程，在估计理论的发展史里，首先提出Kalman filter，但是因为它要求噪声和系统状态都要符合高斯，系统方程为线性，这样就handle不了实际中的大部分问题，因为实际很多问题都是非线性非高斯的，然后很多人就尝试在Kalman的基础上提出别的方法，比如EKF等，后来人们就不求最优估计了，搞个次优估计，其中我们的Particle Filter就是属于这个范畴，通过采样的方法逼近最优解。
那么问题来了，既然是采样，用什么方法采样呢，我们用Monte Carlo方法来采样，那既然用MC方法采样，你也得告诉我们概率分布啊，也就是概率密度函数(也就是贝叶斯滤波理论的后验概率密度函数)，如前面推导所知，理论的后验pdf很难进行采样也很难得到，所以我们就搞了一个重要性采样，重要性采样主要是提出一个重要性概率密度函数，之前不是说后验pdf不好采样不好得到嘛，我们引进来的这个重要性概率密度函数来方便采样(其实这只是我们假设他方便采样，如果不方便采样的话，我们就不用他了)，而且既然是我们引进来的，那必然是我们已知的，进而就有了贝叶斯重要性采样。本来随机变量的后验pdf很难得到，现在贝叶斯重要性采样提出的重要性概率密度函数解决了这个问题。
但是随之而来的一个新问题就是贝叶斯重要性采样呢，计算量比较大，每来一个新数据，重要性概率密度函数就会变化，那么每次采样时，每个粒子的权值就要重新计算，为了简化计算，我们就像前面的递推贝叶斯滤波那样，通过上一个状态的权值来递推当前状态的粒子的权值，这样就会方便很多了，这时候就搞出了一个序贯重要性采样(SIS)解决了这个问题。
按说到这里圆满了，但是呢？又有新问题了，随着时间的推移，许多粒子的权重逐渐逐渐就变成0或很接近与0，而某个粒子的权值接近1，这也就是退化问题，之所以说估计估计，如果只有一个粒子有权值，其他粒子的权值接近为0，这样就不能把粒子的结果作为我们估计的结果，因为样本数量不够啊，就一个粒子不能代表什么情况的，偶然因素太大了，我们希望的是每个粒子都会给我们feedback，这样我们根据大多数的粒子的feedback来估计状态，所以为了解决退化问题，搞了两个办法，一个是选取比较好的重要性密度函数，通过这个选取的粒子不会出现这样的问题，一般这样比较难做到(不知道现在有没有做到的，我目前只是入门PF，所以不太了解)；另外一种常见的办法就是重采样。
重采样，前面说了几种基本的重采样方法，还有比较不错的MCMC方法，前面提到的基本的重采样算法思想就是你权值接近为0了，那你就没有必要存在下去了，你权值大，那你能者多劳，自己再生几个跟你一样的粒子，把权值分一下，也就是物竞天择的意思。这样就能保证能够给feedback的粒子数量充足(这就是自适应重采样)，不像上面提到的最后一家独大了。
所以综合上面说的，就有基本的粒子滤波算法SIR,其重要性密度函数用的就是转移密度函数 $p(x_k|x_{k-1})$ ,然后更新权值，搞个阈值，到一定程度后，就重采样，其重采样算法是多项式重采样。
在举个PF实际应用的粒子，在网上百度的话，最多的就是PF应用在object tracking这一块，那比如说我们用PF算法来做object tracking，那首先告诉我们物体的初始位置，我们去计算这个物体的颜色直方图，然后我们可以按照高斯分布来放置粒子(这也就是理论里的重要性采样，这里pdf就是高斯分布的那个函数)，初始时，给每个粒子分配一样的权重。
接下来就是跟踪的过程，比如经过一帧之后，物体肯定是要动的，但是我们的粒子是不动的，那么我们现在就把各个粒子所在位置的颜色直方图信息提取出来(feature extraction),与我们刚开始计算的颜色直方图进行比较，比如可以使用欧式距离来计算相似度，那越相似的粒子我们给他分配的权重就多一点，越不相似就少一点，(实际编程肯定是要归一化的，不过我还没有编，有时间搞一个)，然后比如说我们取相似度超过前多少的，将他们的位置求平均，最后得到的坐标就认为是当前我们所跟踪物体的新坐标，然后进行重采样，之前按照权值大小进行淘汰和复制，把重采样后的粒子依然按照高斯分布，在那个新坐标的基础上撒出去。
最后这样重复以上步骤，我们就能实现物体的实时跟踪了，当然这是在背景比较简单，跟踪的物体也比较简单，如果你搞的物体和背景颜色很一致，就很难跟踪了，或者说这时候不能用颜色信息做跟踪了，应该提取新的特征来做相似度测量。这个实例就是计算机直觉的范畴了，在其他领域也有应用，我不太了解，就不献丑了。
至此，文章也就结束了，不过我对PF的理解还属于似懂非懂的状态，数学基础不够的人真的很悲剧啊，等过一段时间在回头看看，有没有新的理解。
References
- [1]梁军. 粒子滤波算法及其应用. 哈尔滨工业大学博士论文，2009.6
- [2]Arulampalam, M. Sanjeev, et al. “A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking.“ IEEE Transactions on Signal Processing 50.2(2002):174 - 188.