卡尔曼滤波和粒子滤波

整篇转自：https://blog.csdn.net/zkl99999/article/details/46619771/

---

转自http://blog.csdn.net/karen99/article/details/7771743

卡尔曼滤波本来是控制系统课上学的，当时就没学明白，也蒙混过关了，以为以后也不用再见到它了，可惜没这么容易，后来学计算机视觉和图像处理，发现用它的地方更多了，没办法的时候只好耐心学习和理解了。一直很想把学习的过程记录一下，让大家少走弯路，可惜总也没时间和机会，直到今天。。。

我一直有一个愿望，就是把抽象的理论具体化，用最直白的方式告诉大家--不提一个生涩的词，不写一个数学公式，像讲故事一样先把道理说明白，需要知道细节的同学可以自己去查所有需要知道的一切。因为学习的过程告诉我，最难的其实是最初和这个理论和应用背景亲和的过程--这些理论它究竟是做什么的，又是怎么做到的。可惜我们能看到的关于这些理论的资料大多数都是公式的堆砌并且假定我们明白许多“基本的道理”，其实这些“基本的道理”往往是我们最难想象和超越的。以卡尔曼滤波为例，让我们尝试一种不同的学习方法。

相信所有学习卡尔曼滤波的同学首先接触的都是状态方程和观测方程，学过控制系统的同学可能不陌生，否则，先被那两个看起来好深奥的公式给吓跑了，关键是还不知道他们究竟是干什么的，什么是状态，什么是观测。。。。。。如果再看到后面的一大串递归推导增益，实在很晕很晕，更糟糕的是还没整明白的时候就已经知道卡尔曼滤波其实已经不够使了，需要extended kalmanfilter 和particle filter了。。。

其实我们完全不用理会这些公式。先来看看究竟卡尔曼滤波是做什么的，理解了卡尔曼滤波，下面的就顺其自然了。

用一句最简单的话来说，卡尔曼滤波是来帮助我们做测量的，大家一定不明白测量干嘛搞那么复杂？测量长度拿个尺子比一下，测量温度拿温度表测一下不就完了嘛。的确如此，如果你要测量的东西很容易测准确，没有什么随机干扰，那真的不需要劳驾卡尔曼先生。但在有的时候，我们的测量因为随机干扰，无法准确得到，卡尔曼先生就给我们想了个办法，让我们在干扰为高斯分布的情况下，得到的测量均方误差最小，也就是测量值扰动最小，看起来最平滑。

还是举例子最容易明白。我最近养了只小兔子，忍不住拿小兔子做个例子嘻嘻。

每天给兔子拔草，看她香甜地吃啊吃地，就忍不住关心一下她的体重增长情况。那么我们就以小兔子的体重作为研究对象吧。假定我每周做一次观察，我有两个办法可以知道兔子的体重，一个是拿体重计来称：或许你有办法一下子就称准兔子的体重（兽医通常都有这办法），但现在为了体现卡尔曼先生理论的魅力，我们假定你的称实在很糟糕，误差很大，或者兔子太调皮，不能老实呆着，弹簧秤因为小兔子的晃动会产生很大误差。尽管有误差，那也是一个不可失去的渠道来得到兔子的体重。还有一个途径是根据书本上的资料，和兔子的年龄，我可以估计一下我的小兔子应该会多重，我们把用称称出来的叫观察量，用资料估计出来的叫估计值，无论是观察值还是估计值显然都是有误差的，假定误差是高斯分布。现在问题就来了，按照书本上说我的兔子该3公斤重，称出来却只有2.5公斤，我究竟该信哪个呢？如果称足够准，兔子足够乖，卡尔曼先生就没有用武之地了呵呵，再强调一下是我们的现状是兔兔不够乖，称还很烂呵呵。在这样恶劣的情景下，卡尔曼先生告诉我们一个办法，仍然可以估计出八九不离十的兔兔体重，这个办法其实也很直白，就是加权平均，把称称出来的结果也就是观测值和按照书本经验估算出来的结果也就是估计值分别加一个权值，再做平均。当然这两个权值加起来是等于一的。也就是说如果你有0.7分相信称出来的体重，那么就只有0.3分相信书上的估计。说到这里大家一定更着急了，究竟该有几分相信书上的，有几分相信我自己称的呢？都怪我的称不争气，没法让我百分一百信赖它，还要根据书上的数据来做调整。好在卡尔曼先生也体会到了我们的苦恼，告诉我们一个办法来决定这个权值，这个办法其实也很直白，就是根据以往的表现来做决定，这其实听起来挺公平的，你以前表现好，我就相信你多一点，权值也就给的高一点，以前表现不好，我就相信你少一点，权值自然给的低一点。那么什么是表现好表现不好呢，表现好意思就是测量结果稳定，方差很小，表现不好就是估计值或观测值不稳定，方差很大。想象你用称称你的哦兔子，第一次1公斤第二次10公斤，第三次5公斤，你会相信你的称吗，但是如果第一次3公斤第二次3.2公斤，第三次2.8公斤，自然我就相信它多一点，给它一个大的权值了。

有了这个权值，下面的事情就很好办了。很显然卡尔曼先生是利用多次观察和估计来达到目的的，我们也只能一步一步地调整我们的观察和估计值，来渐渐达到准确的测量，所以整个算法是递归的，需要多次重复调整的。调整的过程也很简单，就是把实测值（称出来的体重）和估计值（书上得来的体重）比较一下，如果估计值比测量值小，那就把估计值加上他们之间的偏差作为新的估计值，当然前面要加个系数，就是我们前面说的加权系数，这个地方我要写个公式，因为很简单就能说明白

比如我们的观查值是Z，估计值是X， 那么新的估计值就应该是 Xnew  =  X  + K ( Z-X），从这个公式可以看到，如果X估计小了，那么新的估计值会加上一个量K ( Z-X）, 如果估计值大了,大过Z了，那么新的估计值就会减去一个量K ( Z-X），这就保证新的估计值一定比现在的准确，一次一次递归下去就会越来越准却了，当然这里面很有作用的也是这个K，也就是我们前面说的权值，书上都把他叫卡尔曼增益。。。(Xnew  =  X  + K ( Z-X） = X ×（1-K) + KZ ，也就是说估计值X的权值是1-k，而观察值Z的权值是k，究竟k 取多大，全看估计值和观察值以前的表现，也就是他们的方差情况了）

发现把一个问题讲明白还真不是件容易的事情，谁听明白了我佩服谁，因为我已经把自己讲糊涂了哈

顺便就把extended kalman filter和particle filter提一下，因为高斯模型有时不适用，于是有了extended kalman filter，而particle filter是用于多个对象的，比如除了兔子我还有只猫，他们的体重有一个联合概率模型，每一个对象就是一个particle。无论是卡尔曼滤波还是particle滤波，都是概率分布传递的过程，卡尔曼传递的是高斯分布，particle filter 传递的是高斯混合分布，每一个峰代表一个动物在我们的例子。

转自：http://www.cnblogs.com/yangyangcv/archive/2010/05/23/1742263.html#undefined

粒子滤波算法

上学的时候每次遇到“粒子滤波”那一堆符号，我就晕菜。今天闲来无事，搜了一些文章看，终于算是理解了。下面用白话记一下我的理解。

问题表述：

某年月，警方（跟踪程序）要在某个城市的茫茫人海（采样空间）中跟踪寻找一个罪犯（目标），警方采用了粒子滤波的方法。

1. 初始化：

警方找来了一批警犬（粒子），并且让每个警犬预先都闻了罪犯留下来的衣服的味道（为每个粒子初始化状态向量S0），然后将警犬均匀布置到城市的各个区（均匀分布是初始化粒子的一种方法，另外还有诸如高斯分布，即：将警犬以罪犯留衣服的那个区为中心来扩展分布开来）。

2. 搜索：

每个警犬都闻一闻自己位置的人的味道（粒子状态向量Si），并且确定这个味道跟预先闻过的味道的相似度（计算特征向量的相似性），这个相似度的计算最简单的方法就是计算一个欧式距离（每个粒子i对应一个相似度Di），然后做归一化（即：保证所有粒子的相似度之和为1）。

3. 决策：

总部根据警犬们发来的味道相似度确定罪犯出现的位置（概率上最大的目标）：最简单的决策方法为哪个味道的相似度最高，那个警犬处的人就是目标。

4. 重采样：

总部根据上一次的决策结果，重新布置下一轮警犬分布（重采样过程）。最简单的方法为：把相似度比较小的地区的警犬抽调到相似度高的地区。

 

上述，2,3,4过程重复进行，就完成了粒子滤波跟踪算法的全过程。

 

一直都觉得粒子滤波是个挺牛的东西，每次试图看文献都被复杂的数学符号搞得看不下去。一个偶然的机会发现了Rob Hess(http://web.engr.oregonstate.edu/~hess/)实现的这个粒子滤波。从代码入手，一下子就明白了粒子滤波的原理。根据维基百科上对粒子滤波的介绍(http://en.wikipedia.org/wiki/Particle_filter)，粒子滤波其实有很多变种，Rob Hess实现的这种应该是最基本的一种，Sampling Importance Resampling (SIR)，根据重要性重采样。下面是我对粒子滤波实现物体跟踪的算法原理的粗浅理解：

1）初始化阶段-提取跟踪目标特征

该阶段要人工指定跟踪目标，程序计算跟踪目标的特征，比如可以采用目标的颜色特征。具体到Rob Hess的代码，开始时需要人工用鼠标拖动出一个跟踪区域，然后程序自动计算该区域色调(Hue)空间的直方图，即为目标的特征。直方图可以用一个向量来表示，所以目标特征就是一个N*1的向量V。

2）搜索阶段-放狗

好，我们已经掌握了目标的特征，下面放出很多条狗，去搜索目标对象，这里的狗就是粒子particle。狗有很多种放法。比如，a)均匀的放：即在整个图像平面均匀的撒粒子(uniform distribution)；b)在上一帧得到的目标附近按照高斯分布来放，可以理解成，靠近目标的地方多放，远离目标的地方少放。Rob Hess的代码用的是后一种方法。狗放出去后，每条狗怎么搜索目标呢？就是按照初始化阶段得到的目标特征(色调直方图，向量V)。每条狗计算它所处的位置处图像的颜色特征，得到一个色调直方图，向量Vi，计算该直方图与目标直方图的相似性。相似性有多种度量，最简单的一种是计算sum(abs(Vi-V)).每条狗算出相似度后再做一次归一化，使得所有的狗得到的相似度加起来等于1.

3）决策阶段

我们放出去的一条条聪明的狗向我们发回报告，“一号狗处图像与目标的相似度是0.3”,“二号狗处图像与目标的相似度是0.02”,“三号狗处图像与目标的相似度是0.0003”,“N号狗处图像与目标的相似度是0.013”...那么目标究竟最可能在哪里呢？我们做次加权平均吧。设N号狗的图像像素坐标是(Xn,Yn),它报告的相似度是Wn,于是目标最可能的像素坐标X = sum(Xn*Wn),Y = sum(Yn*Wn).

4）重采样阶段Resampling

既然我们是在做目标跟踪，一般说来，目标是跑来跑去乱动的。在新的一帧图像里，目标可能在哪里呢？还是让我们放狗搜索吧。但现在应该怎样放狗呢？让我们重温下狗狗们的报告吧。“一号狗处图像与目标的相似度是0.3”,“二号狗处图像与目标的相似度是0.02”,“三号狗处图像与目标的相似度是0.0003”,“N号狗处图像与目标的相似度是0.013”...综合所有狗的报告，一号狗处的相似度最高，三号狗处的相似度最低，于是我们要重新分布警力，正所谓好钢用在刀刃上，我们在相似度最高的狗那里放更多条狗，在相似度最低的狗那里少放狗，甚至把原来那条狗也撤回来。这就是Sampling Importance Resampling，根据重要性重采样(更具重要性重新放狗)。

(2)->(3)->(4)->(2)如是反复循环，即完成了目标的动态跟踪。

 

根据我的粗浅理解，粒子滤波的核心思想是随机采样+重要性重采样。既然我不知道目标在哪里，那我就随机的撒粒子吧。撒完粒子后，根据特征相似度计算每个粒子的重要性，然后在重要的地方多撒粒子，不重要的地方少撒粒子。所以说粒子滤波较之蒙特卡洛滤波，计算量较小。这个思想和RANSAC算法真是不谋而合。RANSAC的思想也是(比如用在最简单的直线拟合上)，既然我不知道直线方程是什么，那我就随机的取两个点先算个直线出来，然后再看有多少点符合我的这条直线。哪条直线能获得最多的点的支持，哪条直线就是目标直线。想法非常简单，但效果很好。

 

扯远了，下面还是说说代码吧。Rob Hess的代码好像是linux上的，我稍微改了下，让Windows+VS2008能跑。main函数在track1.c里，默认是处理附带的视频，当然可以方便的改成从摄像头输入视频(cvCaptureFromCAM)。我用的是opencv2.0，没有测试低于2.0的版本。程序开始后，读入第一帧图像后，需要用鼠标拖动出跟踪目标区域，(然后按回车)。跟踪区域较小时速度较快，跟踪区域大时明显变得很卡。代码还需要gsl库的支持，我用的是gsl1.8,下面提供了gsl1.8安装文件的下载。这里是全部代码和相关的VS2008工程文件

 

 

 

 

 

 

转自：http://skpsun.blog.163.com/blog/static/2760055201010251210841/

这里主要讲计算机视觉范畴下的Kalman Filter（KF）和Particle Filter（PF）。

Kalman Filter建立在高斯模型上的，这是说目标的model，即状态噪音的概率密度函数（state probability density）和观察模型（observation model）是高斯分布。Kalman的数学基础是指，将两个高斯模型结合起来时，将得到一个新的高斯模型，并且新高斯模型的参数（均值和方差）可完全由原来两个高斯模型的参数计算得到。应用在时间序列中，就是说，知道了t-1时刻和t时刻state噪音的分布（符合高斯模型），就可算出t+1时刻state的分布。从而实现迭代运算。

同时，Kalman Filter又是线性的，t时刻的state可以由t-1时刻的state经过一个矩阵相乘得到。

前面提到，噪音是高斯白噪音，白噪音是指其分布和时间不相关。噪音分为两个部分，状态量中的噪音和观测量中的噪音。即分别为下述式（1）的W(t)和和式（2）中V(k)。

所以在高斯和线性两个假设之下，我们既可以得到state的取值（由线性假设得到），又可以得到state取值的概率密度（由高斯假设得到），这时就可以通过迭代实现对系统每一时刻状态的预测。再结合（assimilate）上观测值，对预测结果纠正，从而实现一个完整的跟随预测系统。也就是说，卡尔曼滤波器就是一个信息融合器，把我们不准确的预测结果和不准确的测量结果融合在一起，得到一个估计结果。

以上过程可由一个线性随机微分方程（Linear Stochastic Difference equation）来描述：
                X(t)=F X(t-1)+B U(t)+W(t) -------------------------式（1）
再加上系统的测量值：
                Z(t)=H X(t)+V(t) ------------------------------------式（2）
上 两式子中，X(k)是k时刻的系统状态，U(k)是k时刻对系统的控制量。F（dynamic model或transition model）和B（control model）是系统参数，对于state vector维度高于一的系统，他们为矩阵。Z(k)是k时刻的测量值，H 是测量系统的参数，对于多测量系统，H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise)，他们的covariance 分别是Q，R。这里要注意区分状态量（state）X(t)和观测量（measurement）Z(t)的区别。比如若state是追踪目标的图像位置(x,y)以及速度v，即state vector是一个三维向量，而观测时可能只能对目标的位置进行直接观测，即观测量是(x,y)这样一个二维向量。这两者的不同，体现在矩阵F和H上。

Kalman Filter中的高斯和线性假设有时并不能满足实际情况，因为高斯模型是单峰的（uni-modal）。比如当一个运动目标被遮挡后，我们不知道它将在遮挡物的后面做什么运动，是继续前进，或是停下，还是转向或后退。这种情况下，Kalman Filter只能给出一个位置的预测，至多加大这个预测的不确定性（即增大协方差矩阵，或，若state是单维度时的方差）。这种情况下，预测位置的不确定噪音事实上已不是高斯模型，它将具有多个峰值（multi-modal）。而且，这种噪音常常无法解析表达。这就引入了Particle Filter。

Particle Filter是基于蒙特卡洛方法（Monte Carlo Method），简言之就是用点来表达概率密度函数。点密集的地方，概率密度就大。在时间序列中，就是时间序列蒙特卡洛（Sequential Monte Carlo）。所以Particle Filter在机器视觉中的应用，称为CONDENSATION（Conditional Density Propagation），它和Bootstrap Filter一样，同属于Sequential Monte Carlo范畴。

具体实施上，PF对state的更新不再采用KF中的高斯模型更新，而是采用factored sampling方法。简单说就是对t-1时刻的所有Particle，根据每个Particle的概率，重新对他们采样。高概率的Particle将得到高的采样几率，而低概率的Particle对应低的采样几率。这样，高概率Particle将可能被多次采样，而低概率Particle可能就被放弃。这样得到t时刻的Particle。然后将t时刻每一个Particle所对应的测量值结合起来，为t时刻的Particle重新赋以新的概率，以用于t+1时刻新Particle的生成。

所以可以总结， KF和PF相同的是，都分为三个步骤：Prediction，Measurement和Assimilation（或称为correction）。只是每步的实现上不同。

在Prediction阶段，KF利用高斯模型进行预测，而PF采用factored sampling对原Particle重采样。

在Measurement阶段，KF将得到唯一的measurement，而PF将为每一个Particle得到一份measurement。

在Assimilation阶段，KF将由state计算得到的观测值和实际观测值比较结合，得到更新后的系统参数。而PF通过比较 每个Particle所对应的观测值和模型预测值之间的差别，更新每个Particle的概率。

所以，KF和PF最大的不同就在于对状态概率密度函数的表达方式上。KF采用高斯模型，而PF利用用无参数的点来近似。

参考文献：

1、卡尔曼滤波器：http://www.china-vision.net/blog/user1/6/2006111115624.html

2、Learning OpenCV --- Computer Vision with the OpenCV Library

3、Michael Isard and Andrew Blake，CONDENSATION—Conditional Density Propagation for Visual Tracking，IJCV 1998

4、Michael Isard and Andrew Blake ，Contour Tracking by Stochastic Propagation of Conditional Density，ECCV，1996

5、Aly Merchant, Keir Mierle ，A Simple Implementation of the Condensasion Algorithm，2005

6、Arnaud Doucet ,  Simon Godsill ,  Christophe Andrieu ，On Sequential Monte Carlo sampling methods for Bayesian filtering，Statistics and Computing，2000