粒子滤波 & 重要性采样

粒子滤波（PF：Particle Filter）&与卡尔曼滤波（Kalman Filter）相比较

　　粒子滤波(PF: Particle Filter)的思想基于蒙特卡洛方法(Monte Carlo methods)，它是利用粒子集来表示概率，可以用在任何形式的状态空间模型上。其核心思想是通过从后验概率(观测方程)中抽取的随机状态粒子来表达其分布，是一种顺序重要性采样法(Sequential Importance Sampling)。简单来说，粒子滤波法是指通过寻找一组在状态空间传播的随机样本对概率密度函数进行近似，以样本均值代替积分运算(状态方程)，从而获得状态最小方差分布的过程。这里的样本即指粒子,当样本数量N→∝时可以逼近任何形式的概率密度分布。 　　尽管算法中的概率分布只是真实分布的一种近似，但由于非参数化的特点，它摆脱了解决非线性滤波问题时随机量必须满足高斯分布的制约，能表达比高斯模型更广泛的分布，也对变量参数的非线性特性有更强的建模能力。因此，粒子滤波能够比较精确地表达基于观测量和控制量的后验概率分布，可以用于解决SLAM问题。

粒子滤波的应用

　　粒子滤波技术在非线性、非高斯系统表现出来的优越性，决定了它的应用范围非常广泛。另外，粒子滤波器的多模态处理能力，也是它应用广泛的原因之一。国际上，粒子滤波已被应用于各个领域。在经济学领域，它被应用在经济数据预测；在军事领域已经被应用于雷达跟踪空中飞行物，空对空、空对地的被动式跟踪；在交通管制领域它被应用在对车或人视频监控；它还用于机器人的全局定位。

粒子滤波的缺点

　　虽然粒子滤波算法可以作为解决SLAM问题的有效手段，但是该算法仍然存在着一些问题。其中最主要的问题是需要用大量的样本数量才能很好地近似系统的后验概率密度。机器人面临的环境越复杂，描述后验概率分布所需要的样本数量就越多，算法的复杂度就越高。因此，能够有效地减少样本数量的自适应采样策略是该算法的重点。另外，重采样阶段会造成样本有效性和多样性的损失，导致样本贫化现象。如何保持粒子的有效性和多样性，克服样本贫化，也是该算法研究重点。

粒子滤波的发展

1.MCMC改进策略

　　马尔可夫链蒙特卡洛(MCMC)方法通过构造Markov链，产生来自目标分布的样本，并且具有很好的收敛性。在SIS的每次迭代中，结合MCMC使粒子能够移动到不同地方，从而可以避免退化现象，而且Markov链能将粒子推向更接近状态概率密度函数(probability density function,(PDF))的地方，使样本分布更合理。基于MCMC改进策略的方法有许多，常用的有Gibbs采样器和MetropolisHasting方法。

2.Unscented粒子滤波器(UPF)

　　Unscented Kalman滤波器(UKF)是Julier等人提出的。EKF(Extended Kalman Filter)使用一阶Taylor展开式逼近非线性项，用高斯分布近似状态分布。UKF类似于EKF，用高斯分布逼近状态分布，但不需要线性化只使用少数几个称为Sigma点的样本。这些点通过非线性模型后，所得均值和方差能够精确到非线性项Taylor展开式的二阶项，从而对非线性滤波精度更高。Merwe等人提出使用UKF产生PF的重要性分布，称为Unscented粒子滤波器(UPF)，由UKF产生的重要性分布与真实状态PDF的支集重叠部分更大，估计精度更高。

3.Rao－Blackwellised粒子滤波器(RBPF)

　　在高维状态空间中采样时,PF的效率很低。对某些状态空间模型，状态向量的一部分在其余部分的条件下的后验分布可以用解析方法求得，例如某些状态是条件线性高斯模型，可用Kalman滤波器得到条件后验分布，对另外部分状态用PF，从而得到一种混合滤波器，降低了PF采样空间的维数，RBPF样本的重要性权的方差远远低于SIR方法的权的方差，为使用粒子滤波器解决 SLAM问题提供了理论基础。而Montemerlo等人在2002年首次将Rao-Blackwellised粒子滤波器应用到机器人SLAM中，并取名为FastSLAM算法。该算法将SLAM问题分解成机器人定位问题和基于位姿估计的环境特征位置估计问题，用粒子滤波算法做整个路径的位姿估计，用EKF估计环境特征的位置，每一个EKF对应一个环境特征。该方法融合EKF和概率方法的优点，既降低了计算的复杂度，又具有较好的鲁棒性。 　　最近几年，粒子方法又出现了一些新的发展，一些领域用传统的分析方法解决不了的问题，现在可以借助基于粒子仿真的方法来解决。在动态系统的模型选择、故障检测、诊断方面，出现了基于粒子的假设检验、粒子多模型、粒子似然度比检测等方法。在参数估计方面，通常把静止的参数作为扩展的状态向量的一部分，但是由于参数是静态的，粒子会很快退化成一个样本，为避免退化，常用的方法有给静态参数人为增加动态噪声以及Kernel平滑方法，而Doucet等提出的点估计方法避免对参数直接采样，在粒子框架下使用最大似然估计(ML)以及期望值最大(EM)算法直接估计未知参数。

粒子滤波进展与展望

粒子滤波器的应用领域

　　在现代目标跟踪领域，由于实际问题的复杂性，所面对的更多的是非线性非高斯问题，Hue等把PF推广到多目标跟踪和数据关联，Gordon等对杂波中的目标跟踪问题提出混合粒子滤波器弼，Mcginnity等提出机动目标跟踪的多模型粒子滤波器，Doucet等对跳跃Markov系统状态估计提出了更有效的PF算法 j，Guo把PF用于传感器网络下的协同跟踪 J，Freitas等用PF训练神经网络，Srivastava等把PF用于自动目标识别，Fox等把PF用于移动机器人定位，Ward等提出语音源定位的PF算法’ ，Orton等对来自多个传感器的无序量测提出基于PF的多目标跟踪和信息融合方法，Penny等使用PF实现多传感器资源最优管理和部署，Hernandez等结合PF、数据融合和优化算法实现多传感器资源管理．研究表明PF是解决此类非线性问题的有力工具之一．PF在计算机视觉、可视化跟踪领域被称为凝聚算法(CONDENsATION)，该领域是PF的一个非常活跃的应用领域，Bruno提出图像序列中目标跟踪的PF算法，Maskell等提出基于图像传感器多目标跟踪的PF算法_4 ．在听觉视觉联合目标定位和跟踪方面，Vermaak等利用PF提出声音和视觉融合的集成跟踪，Zotkin等使用PF将来自多个摄像机和麦克风组的视觉听觉信息融合跟踪移动目标． 　　在粒子滤波算法下一些传统的难点问题如目标检测、遮挡、交叉、失跟等得到更好的结果．在无线通讯中PF被广泛用于信道盲均衡、盲检测、多用户检测等方面．其它的应用领域还有机器人视觉跟踪、导航、图象处理、生物信息引、故障诊断和过程控制、金融数据处理等．研究表明在有关非高斯非线性系统的数据处理和分析领域PF都具有潜在的应用价值．值得一提的是国内学者在PF的研究上也取得许多成果，莫等利用PF算法提出一种混合系统状态监测与诊断的新方法，Chen等利用PF预测非线性系统状态分布，获得故障预测概率，Li等提出基于PF的可视化轮廓跟踪方法 J，Shan等提出基于PF的手形跟踪识别方法，Hu等提出闪烁噪声下的PF跟踪算法等，这些工作推动PF在国内的研究．

粒子方法的新发展

　　粒子滤波器采用一组随机粒子逼近状态的后验概率分布，有可能用粒子逼近平滑分布，由于重采样使得粒子丧失多样性，直接由滤波分布边缘化得到的平滑分布效果很差，Doucet等应用MCMC方法增加样本多样性用于固定延迟平滑取得好的效果，Fong等把RBPF推广到粒子平滑器，并用于语音信号处理． 　　在PF的性能优化方面，目前大多优化某个局部的性能指标，如重要性权的方差等，Doucet等使用随机逼近对PF关于某个全局性能指标进行在线优化，Chan等人进一步利用SPSA随机优化方法优化PF ，避免1r梯度的计算．为了减少计算量使得PF能用于实时数据处理，Foxt提出了粒子个数可变的自适应粒子滤波器，Kwok等把粒子划分为小的集合，每个小样本集可以进行实时处理，采用加权和的方法逼近状态后验分布，Brun等提出PF的并行结构算法以获得在线实时应用．　　最近几年，粒子方法出现了又一些新的发展，一领域用传统的分析方法解决不了的问题，现在可以借助基于粒子仿真的方法来解决．在动态系统的模型选择，故障检测、诊断方面，出现了基于粒子的假设检验、粒子多模型、粒子似然度比检测等方法．在参数估计方面，通常把静止的参数作为扩展的状态向量的一部分，但是由于参数是静态的，粒子会很快退化成一个样本，为避免退化，常用的方法有给静参数人为增加动态噪声_9 以及Kernel平滑方法，而Doucet等提出的点估计方法避免对参数直接采样，在粒子框架下使用最大似然估计(ML)以及期望值最大(EM)算法直接估计未知参数．在随机优化方面，出现了基于粒子方法的梯度估计算法，使得粒子方法也用于最优控制等领域．Andrieu，Doucet等在文献[70]中详细回顾了粒子方法在变化检测、系统辨识和控制中的应用及理论上的一些最新进展，许多仅仅在几年前不能解决的问题现在可以求助于这种基于仿真的粒子方法．

总结与展望(Summarization and prospect)

　　目前粒子滤波器的研究已取得许多可喜的进展，应用范围也由滤波估计扩展到新的领域，作为一种新方法，粒子方法还处于发展之中，还存在许多有待解决的问题，例如随机采样带来Monte Carlo误差的积累甚至导致滤波器发散、为避免退化和提高精度而需要大量的粒子使得计算量急剧增加、粒子方法是否是解决非线性非高斯问题的万能方法还值得探讨．此外粒子滤波器还只是停留在仿真阶段，全面考虑实际中的各种因素也是深化PF研究不可缺少的一个环节．尽管如此，在一些精度要求高而经典的分析方法又解决不了的场合，这种基于仿真的逼近方法发挥了巨大潜力，而现代计算机和并行计算技术的迅速发展又为粒子方法的发展和应用提供了有力支持，相信粒子滤波器的研究将朝着更深，更广的方向发展．

扩展阅读： 卡尔曼滤波

 

 

 

粒子滤波算法源于Montecarlo的思想，即以某事件出现的频率来指代该事件的概率。因此在滤波过程中，需要用到概率如P(x)的地方，一概对变量x采样，以大量采样的分布近似来表示P(x)。因此，采用此一思想，在滤波过程中粒子滤波可以处理任意形式的概率，而不像Kalman滤波只能处理高斯分布的概率问题。他的一大优势也在于此。

再来看对任意如下的状态方程

x(t)=f(x(t-1),u(t),w(t))

y(t)=h(x(t),e(t))

        其中的x(t)为t时刻状态，u(t)为控制量，w(t) 和e(t)分别为模型噪声和，观测噪声。前一个当然是状态转移方程，后一个是观测方程。那么对于这么一个问题粒子滤波怎么来从观测y(t)，和x(t-1),u(t) 滤出真实状态x(t)呢？

        看看滤波的预估阶段：粒子滤波首先根据x(t-1) 和他的概率分布生成大量的采样，这些采样就称之为粒子。那么这些采样在状态空间中的分布实际上就是x(t-1) 的概率分布了。好，接下来依据状态转移方程加上控制量可以对每一粒子得到一个预测粒子。所有的预测粒子就代表了涉及哪些参数化的东西）。

        进入校正阶段来：有了预测粒子，当然不是所有的预测粒子都能得到我们的时间观测值y对不，越是接近真实状态的粒子，当然获得越有可能获得观测值y对吧。于是我们对所有的粒子得有个评价了，这个评价就是一个条件概率P(y|xi),直白的说，这个条件概率代表了假设真实状态x(t)取第i个粒子xi 时获得观测y的概率。令这个条件概率为第i个粒子的权重。如此这般下来，对所有粒子都进行这么一个评价，那么越有可能获得观测y的粒子，当然获得的权重越高。好了预测信息融合在粒子的分布中，观测信息又融合在了每一粒子的权重中。

        哈哈最后采用重采样算法，去除低权值的粒子，复制高权值的粒子。所得到的当然就是我们说需要的真实状态x(t)了，而这些重采样后的粒子，就代表了真实状态的概率分布了。

        下一轮滤波，再将重采样过后的粒子集输入到状态转移方程中，直接就能够获得预测粒子了。。

        初始状态的问题：咱们一开始对x(0)一无所知对吧，那咱们就认为x(0)在全状态空间内平均分布呗。于是初始的采样就平均分布在整个状态空间中。然后将所有采样输入状态转移方程，得到预测粒子。再评价下所有预测粒子的权重，当然我们在整个状态空间中只有部分粒子能够获的高权值咯。马上重采样算法来了，去除低权值的，将下一轮滤波的考虑重点立马缩小到了高权值粒子附近。

 

重要性重采样 Sampling Importance Resampling粒子滤波中的一个采样方法

　　传统分粒子滤波器应用顺序重要性采样( sequential importance sampling, SIS) ,会引起粒子退化, 因为随着循环的运行,一些粒子越来越重要,规一化的权重逐渐趋向1,而另一些权重趋向0,这样大量低权重的粒子就被从粒子集里删掉了,为了避免这种退化, 有研究者应用重要性采样重新采样(SIR ) , 忽略低权重粒子,对高权重粒子不断复制,这种方法的一个极限情况是粒子集中仅包含一个特定的粒子和它的所有复制,引起严重的粒子耗尽问题。

 

简单地说，就是根据粒子的权重去掉权值小的粒子，留下权值大的粒子。最基本的思想就是用产生（0，1）的均匀分布，然后看随机数落到哪个粒子上，权重大的粒子被选中的概率也大，选中一次该粒子就会被复制一次。

 

 

重要性采样是非常有意思的一个方法。我们首先需要明确，这个方法是基于采样的，也就是基于所谓的蒙特卡洛法(Monte Carlo)。蒙特卡洛法，本身是一个利用随机采样对一个目标函数做近似。例如求一个稀奇古怪的形状的面积，如果我们没有一个解析的表达方法，那么怎么做呢？蒙特卡洛法告诉我们，你只要均匀的在一个包裹了这个形状的范围内随机撒点，并统计点在图形内的个数，那么当你撒的点很多的时候，面积可以近似为=(在图形内的点的个数/总的点个数)，当你撒的点足够多的时候，这个值就是面积。这里假设我们总有办法（至少要比找解析的面积公式简单）求出一个点是否在图形内。另一个例子，如果你要求一个稀奇古怪的积分，没有解析办法怎么办？蒙特卡洛法告诉你，同样，随机撒点，你一定可以知道f(xi)的值，那么这个积分的解可以表示为=(b-a)/点的个数*sigma[f(xi)]，其中b,a为积分的上下限。

好了，知道了蒙特卡洛法，下面来说重要性采样的前提一些内容。

很多问题里，我们需要知道一个随机变量的期望E(X)，更多时候，我们甚至需要知道关于X的某一个函数f(X)的期望E[f(X)]。问题来了，如果这个X的概率分布超级特么的复杂，你准备怎么做呢？积分么？逐点求和么？听上去挺不现实的。这时蒙特卡洛法跑出来告诉你，来来来，咱只要按照你这个概率分布，随机的取一些样本点，再sigma(p(xi)*f(xi))不就可以近似这个期望了么。但问题又来了，你怎么”按照这个概率分布“去撒点呢？

经典蒙特卡洛法是这么做的，首先把这个概率分布写成累计概率分布的形式，就是从pdf写成cdf，然后在[0,1]上均匀取随机数(因为计算机只能取均匀随机数)，假如我们取到了0.3，那么在cdf上cdf(x0)=0.3的点x0就是我们依据上述概率分布取得的随机点。

举个具体例子吧，例如我想按照标准正态分布N(0,1)取10个随机数，那么我首先在[0,1]上按照均匀分布取10个点

0.4505    0.0838    0.2290    0.9133    0.1524    0.8258    0.5383    0.9961   0.0782    0.4427

然后，我去找这些值在cdf上对应的x0，如下

-0.1243   -1.3798   -0.7422    1.3616   -1.0263    0.9378    0.0963    2.6636  -1.4175   -0.1442

那么上述这些点，就是我按照正态分布取得的10个随机数。

OK，你按照上述方法去找cdf吧。

因为，如果概率分布都超级特么的复杂，累计概率分布岂不是会更特么不知道怎么求了！

 

然后，我们开始了重要性采样的介绍。

让我们回顾一下期望的求法E(f(x))=sum( p(x) * f(x) ) dx。那么，现在我们引入另一个概率分布s(x)，相比于p(x)，s(x)是非常简单能找到cdf的。那么我们变形一下E(f(x)) = sum( p(x) * f(x) / s(x) * s(x) ) dx ，再仔细看看，这个求f(x)的期望变成了，求在s(x)分布下，p(x)*f(x)/s(x)的期望。

重要性采样的关键就在这里，把对f(x)不好求的期望，变成了一个在另一个分布下相对好求的期望。

这样，s(x)能找到cdf，那么就用上面提到的那个方法去采样，然后对应的，求出h(x0)=p(x0)*f(x0)/s(x0)的值，最后再sigma(s(xi)*h(xi))就可以近似E(f(x))了。

举个例子：就上面那个求积分的问题，用重要性采样解释还可以有很好玩儿的内容。上面求积分时，我们是用的均匀采样的方法，注意这个时候自变量X已经被我们弄成了随机变量，f(X)就是这个随机变量的函数。但是，大家可能会注意到这个问题：如果这个f(x)长的比较特别，例如是个高斯函数N(a,b^2)，只不过它的方差b特别的小，但是自变量范围特别大。这时的均匀采样，大多数点都会落在了概率很低的地方，落在a附近的点很少。这样，均匀随机采样法得到的期望很有可能会和真实值差得非常远。（唉，这个问题想不明白的画图，还想不明白的做实验）。那么，此时，如果我们换一个概率，不用均匀采样法，用，例如说N(a,b^2)分布，用上述方法重要性采样一下。那么落在a附近的点会超级的多，这样，得到的期望会很好的近似真实值。

当然，上面那个分布是我随口说的，大家都希望那个重要性采样的概率函数可以无限的逼近真实分布。但既然能表示真实分布，我们就知道cdf了，谁还需要重要性采样呢？所以这只是理论情况。实际上，一般大家用的方法都会根据具体的情况选择。我所见到的，大多都是利用某一种距离/相似度度量函数，然后把这些距离利用某种方法变换成概率分布。这么说还是太抽象，举例吧：

我有一个特定人的模板，我希望在一个给定的区域内寻找这个人。那么粒子的状态就是位置坐标(x, y)和大小(w,h)，每个粒子的权重：

首先，求这个粒子的直方图，再和模板求一个距离，巴式距离啦，EMD啦，随你选。假设这个值为x。

然后，计算K*exp^(-alpha*x)。这个方法被称为likelihood map，就是说分数越小则概率越高，分数越大概率越低。反正K和alpha积分从0到正无穷的和是1就可以了。这样每个点都有了一个概率值。

且慢，现在还不是概率值。所有粒子的和不是1，所以只能叫权重值。然后再归一化一下，就成为了概率值。

最后这个值就是我们要找的s(x)。p(x)，f(x)，s(x)都有了，这样我们就可以比较轻易的利用s(x)的分布撒点，求期望了。

 

当然，在很多文章里这些概率都是带着条件概率的，有的利用马尔科夫性，只和前一帧的状态以及观测相关，有的则写成和以前全部状态相关。但是原理基本是一致的。s(x)的选取也各不相同，具体问题具体分析了。

 

http://hi.baidu.com/matrix549/blog/item/4ee25a91d0506658d1135e98.html

 

 

 

 

重采样中在获得了某一区间里的统计数 N，然后将对应区间的状态 X 复制 N 次就使得到其服从后验概率密度了，这怎么理解了？

所谓重采样即粒子的重新分配过程. 也就是说像分配初值时取平均是最简单的方法了；

因为在粒子滤波发展之前是序贯门特卡罗（SMC），不采用重采样的结果可能因为少数的粒子在高后验分布区域，耗费计算量

 

粒子滤波的几个步骤，一，初始化，从先验概率中随机抽取粒子，权值均为１／Ｎ，二，重要性采样和权值更新，这里的采样是从转移概率密度函数中抽取，权值更新，进行归一化，三，重采样，如果ＮＥＦＦ＜Ｎ，那么进行重采样，重新赋权值为１／Ｎ；

１，初始化既然已经确定粒子及权值了，为什么还要在第二步进行重要性采样和权值更新，那么初始化就没有任何意义了，要是这样的话还不如直接在第一步进行第二步的内容；

２，在重采样的过程中，它是对粒子全部进行一次抽取然后赋权值为１／Ｎ（或者权值从后验概率中得到），还是只将权值较小的粒子去掉，此时粒子应该如何补充．

３，如果用ＵＰＦ时，第一步和第三步不变，主要是第二步，此时可以得到Ｎ个粒子的均值及协方差，那么如何选择重要性采样及权值更新？

第三步对每个粒子用UKF得到的滤波值及其协方差，其中滤波均值作为更新的粒子每个粒子的协方差阵求(det(P))^(-1/2)，以此作为其权值，然后规一化，别的步骤和PF是一样的，但我算出的结果有差别很大，估计哪有问题

 

初始化是生成初始粒子，代入预测方程后，得到下一时刻的粒子，此时要根据观测进行权值更新，重采样是因为权值退化所以用重采样，重采样N个粒子，权大的代替了权值小的粒子!!!!!!!!!!!!!!!!!!!!!

粒子滤波的重采样方法

 

 

粒子滤波和Kalman的模型是一样的。无论是线性还是非线性，它俩的模型都是那个：

z_t=h(x_t, v_t);

x_t=f(x_(t-1),w_t);

虽然粒子滤波中，不太常提这个表达形式，因为大家都默认你懂了Kalman。因此，粒子滤波的关键不在这个模型，而在于一个假设：

P(x_t| z_t)不再是高斯分布了！

前面已经反复强调过，在Kalman下，状态的后验概率是一个高斯分布，也就是一个单峰概率分布。这种模型最大的问题是：无法处理非高斯模型。形象一些说，如果你发烧了，那你既有可能是嗓子发炎了，也有可能是身体其他部分发炎了，甚至有可能产生肿瘤了。那对于发烧这个现象的原因的条件概率，就会有多个峰值。但如果使用Kalman，则假设了高斯分布，自然无法正确表示这个模型。

那么Particle Filter是怎么搞的呢？它的关键核心在于，他用一堆状态例子来表示这个不知道该怎么解析表示的后验概率。

其实这件事情也好理解，对于一个概率分布y=p(x)而言，你取一些x，让x对应它的概率值p(x)，写成点对就是(x,p(x))。那我问，x=x0时，p(x)是多少啊？你自然回答p(x0)... 看似很傻的，其实PF就是这意思。

那本来能取到的值就是有限个，那对于没能取到的x0，如何找到对应的p(x0)呢？一般大家给出的答案是p(x0)=1/n sigma[w_i*diracdelta(x_i-x0)] ，其中(x_i,w_i)就是我们取的点对，那个diracdelta就是冲击函数。话说，我个人以为，其实用非参估计一下就能模拟任何一点的概率。

在做预测的时候，如果你想知道当前观测值下，最有可能的状态是啥，你当然需要找到p(x0)的最大值，但，问题是，谁能用离散的方法遍历一遍状态空间，再找到最大值呢？太特么慢了。所以，一般的做法，就是选择取到的点里面w_i最大的的那个x_i作为预测状态。

但通常情况没有我么想的这么简单。最恶心的情况是，我们无法直接得到P(x_t|z_t)。那么该怎么解决这个问题呢？

于是引入一种叫做importance sampling重要性采样的概念。这个概念是要求解不知道概率分布的E(x_t)，那么它的逻辑在于，另外给一个变量L，它的概率好求，并且已知E(L)=1。那么，鉴于两者是独立的E(x_t/L)=E(x_t)/E(L)=E(x_t)，可是这个时候，我们就可以依据L的分布，求x_t/L这个变量的期望了。【这里需要注明，我的理解很有可能是错的】

至于这个函数到底怎么写出，我也不知道，我还没能写出一个来。再多的细节，我还没能理解，简单的情形就是，有的人用其他的概率来代替这个后验概率。于是，我们就可以得到某一个粒子群(x_i, w_i)，其中w_i不是后验概率，而是某个相关概率。例如状态和测量值的关系等等。

这套框架建立起来后，就剩下更新一类的细节了。

首先，每个粒子在下一时刻开始的时候，都需要自己被预测一下，就是利用最初的那个模型，x_i_(t+1) = f(x_i_t, w_t); 于是，原来Kalman的一步，现在成了n步，每一个粒子都是一个Kalman。

给定观测值z_t后，就需要更新一下每个点的权值，w_(i+1) = w_i * f(神马玩意的)。这里有个东西，就是w_i也是迭代更新的，啊啊啊，具体证明我还得看看。 然后再把w_(i+1) normalize（归一化）一下。

那正经的预测真值是神马那？对的，就是后验概率的期望。（记得原来Kalman直接求了一个值，可那个值恰好是期望啊）。也就是：

x^_(t+1) = sigma[w_(i+1)*x_i_(t+1)]

整个框架到此就结束了。

然则，这个方法存在了一些新问题。在不断更新的过程中，不靠谱儿的粒子的权值会越来越小，最后忽略不计，只有个别几个粒子权值超大，于是就稳定在了某一个区域内。这就是传说中的退化。

解决这个问题，就提出了resampling，重采样。其实，重采样也不难理解。传说中的monte carlo(蒙特卡洛)算法管用了。通常，我们的随机数都是均匀分布取出来的。要是想得到非均匀分布的采样，如何利用均匀分布呢？就是把非均匀分布的点，按照概率的大小重复复制，让每个点的概率一样。例如某个离散分布(1,0.3), (2,0.5),(3,0.2)，那么想要依照此概率采样，就把这个分布伸展成一个数列(1,1,1,2,2,2,2,2,3,3)，这样再均匀采样的时候，得到的就是上述离散分布的采样。于是，对于我们原来的问题，只要求出累计概率分布，或者干脆也用上述办法，把每个粒子按照它的权值展开成数组，再均匀取样就好了。

再然后，可能出现的问题是，重采样过后，很多粒子会一模一样，他们再用模型预测下一步时也会一模一样，不解决问题啊。有人就提出diffuse，就是再给他们加一个随机量，在附近分散一下粒子。

这就是目前粒子滤波的问题以及解决办法。

 

 

 

 

什么是粒子滤波

    粒子滤波(PF: Particle Filter)[1]算法来源于蒙特卡洛方法(Monte Carlo method)，它是利用粒子集来表示概率，可以用在任何形式的状态空间模型上。其核心思想是通过从后验概率中抽取的随机状态粒子来表示其分布情况，是一种顺序重要性采样法(Sequential Importance Sampling)。

1.       初始化阶段

在跟踪前需要选定出要跟踪的目标物体，这个过程可以用人工划定方法和自动识别方法。使用人工的方法可以通过鼠标在图像区域标记出一片感兴趣矩形然后计算它的特征；使用自动的方法就是目标检测技术，识别出图像中要跟踪物体的大致位置。

对于粒子滤波的初始化过程我们可以假设成为孕育美女的过程，我们既可以像毛泽东所提倡的那样，全国各地各种人物生出一堆子女(这里的子女即是particle)来，不管美丑------在图像中到处放粒子；也可以人为的指定哪几家豪门贵族太子党才可以生子女------在指定的ROI中放粒子；更或者我们采取公投的方式，选出我们心目中真正的美女世家来制造子女------模式识别的方法。这些采样出来的粒子就是对当前后验概率的一种近似。

2.       转移阶段

使用粒子滤波算法来对目标进行跟踪，即是通过前一次的先验概率来估算出当前环境下的后验概率密度，这个过程也是由粒子来完成的。接着上面的例子来解释，那些有资格造小人的夫妻要开始造小人了(本人没有造过，YY一下)，众所周知，这些生出来的小孩是不可能完全跟父母完全一样的，不管是长相、身高还是性格，但是他们却保留着父母的大部分基因。粒子也一样，当前粒子的分布不可能跟上一帧时的分布一模一样，但是他们确实应该分布在上一帧位置的大致周围，这个变异过程就叫作转移。

粒子滤波的转移方程[2]跟Kalman滤波的差不多：

 

 

上面的是状态转移方程，下面的为观测方程，wk和vk是高斯噪声。但是有个问题希望有研究过研究的师兄们指点：其实在算法的实现中并不是用的这个方程，Kalman滤波也是一样，而是用的1阶或者2阶自回归方程，使用2阶自回归的转移函数代码如下：

particle transition( particle p, int w, int h, gsl_rng* rng )

{

         float x, y, s;

         particle pn;

        

         x = A1 * ( p.x - p.x0 ) + A2 * ( p.xp - p.x0 ) +

                   B0 * gsl_ran_gaussian( rng, TRANS_X_STD ) + p.x0;

         pn.x = MAX( 0.0, MIN( (float)w - 1.0, x ) );

         y = A1 * ( p.y - p.y0 ) + A2 * ( p.yp - p.y0 ) +

                   B0 * gsl_ran_gaussian( rng, TRANS_Y_STD ) + p.y0;

         pn.y = MAX( 0.0, MIN( (float)h - 1.0, y ) );

         s = A1 * ( p.s - 1.0 ) + A2 * ( p.sp - 1.0 ) +

                   B0 * gsl_ran_gaussian( rng, TRANS_S_STD ) + 1.0;

         pn.s = MAX( 0.1, s );

         pn.xp = p.x;

         pn.yp = p.y;

         pn.sp = p.s;

         pn.x0 = p.x0;

         pn.y0 = p.y0;

         pn.width = p.width;

         pn.height = p.height;

         pn.histo = p.histo;

         pn.w = 0;

         return pn;

}

3.       决策阶段

转移阶段可以理解为父母基因的结合或突变，使得生出来的小孩具有多样性，却又不失相似性。但是生出来的到底是不是美女，这就要求有个决策过程，要砖家们给她打个分。决策的方法根据不同的需求会不同，例如代码中使用距离作为衡量的标准，同时计算出来的相似度或者说分数就是相应粒子的权重。每一个粒子都需要计算其权重，并且需要将其归一化。

4.       重采样阶段

这样就出现了一个问题，如果某对夫妻生的女儿分数不高怎么办？他们的女儿是否会悲剧？现实是残酷的，在资源有限的情况下，如果你们生不出美女那么对不起，就让其他生出美女的家庭来生更多吧，而且是当场再生。

粒子滤波算法会淘汰权值低的粒子，让权值高的粒子来产生出更多的粒子，这就使得算法朝着权值高的地方收敛。假设有200个粒子，1号粒子的权重为0.01而2号粒子的权重为0.004。于是在重采样阶段，1号粒子生孩子的指标是0.01×200=2，2号粒子的指标是0.004×200=0.8，可以发现，1号粒子除了刚产生的粒子外还要再额外的产生一个粒子，而2号粒子就悲剧了，你再没有机会了[3]。如此，最后得到的200个粒子即为所求，然后取个加权平均就得到了目标的状态值。

一个Linux下的C程序和一个VS2008下的C程序分别参见[4][5]。

 

http://blog.csdn.net/yanqingan/archive/2010/11/27/6039853.aspx

参考文献：

[1]     

[2]     

[3]      S. M. Antonio, “Detection and tracking of humans for visual interaction,” PhD Thesis, UK, 2005.

[4]      http://blogs.oregonstate.edu/hess/.

[5]      http://www.cnblogs.com/yangyangcv/archive/2010/05/23/1742263.html.

 

贝叶斯推理 http://blog.sina.com.cn/u/1265634380

 

 

 

 

 

 

 

 

 

 

 

 

~welch/kalman/media/pdf/kalman_intro_chinese.pdf

所谓滤波，实际上是要去掉自己不想要的信号，保留想要的部分。一般来说，是把过程中的噪声去掉。

卡尔曼滤波的默认假定是，世界充满噪声，任何测量结果都有噪声，状态转移过程会有噪声，你想知道系统的真实值么？玩儿蛋去吧。

卡尔曼滤波另一个重要假定模型是这样的，一个系统会处在各种不同的状态，并且会在状态之间转化来转化去。但是呢，倒霉的是我们谁也不知道该系统当前到底是在什么状态；但是呢，幸运的是我们可以通过测量的结果猜测到系统当前在一个什么状态。

那啥叫状态呢？例如系统的速度，加速度，温度，脑残度都算。离散系统的话，我们可以假设一个黑盒，黑盒里有许多颜色的灯（红橙黄绿青蓝紫），同时只能有一个颜色在亮着，ok，哪个灯在亮就是当前状态。

 

下面就是建模：

z_t = H*x_t + v_t;   (1)

x_t = A*x_(t-1) + B*u_(t-1) + w_(t-1);  (2)

初看起这俩式子来，我也头大，不过稍微分析一下也不太难。x_t是t时刻系统所在状态，z_t是所谓观测值，u_t是系统控制变量（已知，但我做的领域一般用不着），w_t , v_t都是噪声。

那么式子(1)就是想说，观测值和系统状态的关系：如果你看到种子发芽了，那么它的状态就是刚出生；如果你看到它开始长叶儿了，那状态就叫生长期；如果丫开花了，就叫啥啥期；如果结果了，就叫成熟期；如果蔫儿了，就叫ddd期。

哦，对了，个人理解一下，以上公式限定为了线性系统，传说中卡尔曼滤波是线性的，来源就在这里了，谁叫你是矩阵乘向量呢，你要是写成f(x_t)，那有可能就是非线性的了。

那么式子(2)就是说，前一时刻状态和下一时刻状态之间的关系。我在这里卡了好久，总是以为丫是马尔科夫过程，所以就完全无法理解A这个系数是凭啥得来的。其实，就是一个固定的转移方程，该方程完全没有概率问题。

所以！以上式子中，固定下来的是H, A, B，这三个矩阵千年不变，万年不变，并且是事先设定好的，全都已知。未知的话....你自己编一个模型吧。 那么w_t，v_t 在这里限定为两个高斯白噪声N(0, Q)和N(0, R)。哦，对，这里要记得，Q,R都tm是协方差矩阵啊，因为，系统状态和观测值都是向量。我对协方差可郁闷可郁闷了。俩随机变量独立，协方差一定为0。

卡尔曼滤波，本质是想要预测下一步的观测值，或者实时监控一下系统所在状态。但一般在我这个领域，大家还都是在玩儿预测，那咱就从预测角度分析。OK，直觉上，给定上一个位置的状态x_(t-1)，式子(2)足够了。但是，回到开始的默认假设，式子(2)得到的结果x^-_t那是各种不准确啊。不准确怎么办？那就去看观测值呗，于是得到观测值z_t，但是观测值也不准确唉，那怎么办？当当当当，卡尔曼告诉我们，一个相对准确的系统值具有如下结构：

x&_t = x&-_t + K( z_t - H*x_(t-1) );  (3)

提一下，这里" & "表示估计值，" - "表示是用前面式子算出来的估计值，不带" - "表示是最后的估计值。 (3)这个式子是想说，你不是式子估计和观测值都不准么，那么你把他俩加个权，求个和，那就可能更准确了。啥？你说这个式子不像加权？你把丫拆了，倒腾倒腾不就像了。所以，最牛B就牛B在了这个“K”，传说中的卡尔曼增益。这个K怎么得到的？我也不知道。文章说法是，定义误差 e_t = x_t - x&_t ，P_t为此误差的协方差矩阵，目的是使这个误差协方差矩阵最小化，把(3)代过去，于是折腾来折腾去，再求个导数为0，解得K，这个关键值的算法：

K = P-_t * H^T * ( H * P-_t * H^T + R) ^(-1);  (4)

哦，对了，另外注意一点，从此以后，我们就都在估计上做文章了，你只要记得，咱永远看不到真实值就行了，于是我们的式子里不是带"&"就是带"-"。

那么式子(4)就是在说，你丫这么着就把K求出来了。于是，问题就变成了这个P-_t怎么个求法。

说到这里，传说中的卡尔曼滤波就算讲完了。神马？你说P-_k还没求呢。是啊，卡尔曼滤波一共就俩需要求的玩意儿，还都tm是迭代求解，一个就是这个P-_t，另一个就是状态x-_t。你随便给个初始值，然后就迭着吧。

言归正传，我还得给出迭代的公式：

x-_t = A * x&_(t-1) + B * u_(t-1);  (5)

P-_t = A * P_(t-1) * A^T + Q;    (6)

大家一定别搞混Q和R谁是哪个公式冒出来的啊。另外严重关切一下这里"-","&"以及不加的关系。注意到啥没有？对了，(6)式中等号右边的P_(t-1)不带任何符号，嘿嘿，那自然是还差一个公式啦：

P_t = (I - K_t * H ) P-_(t-1);   (7)

大功告成，以上就是传说中的卡尔曼滤波的迭代求解方法，如果你要预测状态呢，就用式子(5)的结果；如果预测观测值呢，就把式子(5)的结果乘个H；如果是监测状态呢，就用式子(3)的结果。

 

最后小注一下，文章指出，如果Q,R是常数，K会收敛，也即P也会收敛到常量。另外，大家经常诟病卡尔曼滤波都是假定高斯分布，我勒个去，这里的高斯分布到底说谁呢？噪声项？虽然看上去应该是，但我打赌不是。可是其它又都是定值，唉，头大。我本来就是为了理解这句话才来学习卡尔曼滤波的。

PS, 于是，果然，文中提到x_t是一个随机变量，并且在已知z_t的情况下 p(x_t | z_t) 服从N( x&_t, E[(x_t - x&_t)(x_t - x&_t)])，切记切记，这里所说的正态分布，是指已知观测值的情况下，系统状态是一个高斯分布的随机变量。