为什么要有纸，因为懒得背书；

为什么要有印刷术，因为懒得抄书；

为什么要有指南针，因为懒得记路；

为什么要有蒸汽机，因为懒得做活；

为什么要有交通工具，因为懒得走路；

为什么要有银行，因为懒得管钱；

为什么要有计算机，因为懒得计算；

为什么要有。。。

因为这个世界是懒人的世界，这个世界也是由懒人创造的

这次是纯粹模仿一个小印的想法：http://danielbaggio.blogspot.com/2008/03/enhanced-version-of-head-tracking-and.html

刚开始效果并不好，会出现很多勿检导致角度判断出错，以至于我一度准备着手kalman filter了。后来发现在使用adaboost算法时，haar分类器可以使用CV_HAAR_FIND_BIGGEST_OBJECT这个参数而不是传统的CV_HAAR_DO_CANNY_PRUNING，准确度和效率都可以得到很大的提高。

但是无论如何，我这个的效率ms还是比那个小印高的

adaboost的鲁棒性确实不错，一般的遮挡和运动不会产生明显影响。

不过这个还尚未加上滤波，而且我的目标也不仅仅限于此，xx同学介绍的TrackerIR是终极目标

如此一来，本科毕业设计某些知识可以派上用场了

我们每个人的故事就像沙滩上的脚印，即便不会终究被历史大海所磨平，也会被后人的脚印所掩埋；

如果我们有像海水一样的傻劲，那么，再尖锐的礁石也能被磨平；

在污浊的东西，历史的大海竟然也能给冲刷的一干二净；

之前一直在做移动机器人定位算法。查来查去，发觉粒子滤波算法（又叫MC算法）应该算是最流行的了。因此开始学习使用之。入手的是本英文书叫 “probalistic robotic” 很不错，我所见到的讲得最好的一本书。花了大量时间去研读。在这里我想谈谈我对粒子滤波的一点认识。因为在这一领域算是个新手。希望有前辈或者达人来指正我的想法。也希望我的这篇文章对新手有理解他有所帮助（当初我就很是苦于它难于理解）在这里我不想谈粒子滤波的理论基础和推到，这点大家可以去自己翻书。我只谈下我的体会。

粒子滤波算法。他源于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|xⁱ),直白的说，这个条件概率代表了假设真实状态x(t)取第i个粒子xⁱ  时获得观测y的概率。令这个条件概率为第i个粒子的权重。如此这般下来，对所有粒子都进行这么一个评价，那么越有可能获得观测y的粒子，当然获得的权重越高。好了预测信息融合在粒子的分布中，观测信息又融合在了每一粒子的权重中。

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

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

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

明白了没？没看糊涂吧哈哈。

如果大家看得还不过瘾，后面有根据精彩的论述。

如果跟kalman滤波相比，那确实。毕竟kalman滤波可以直接得到状态的解析估计，计算量很小。如果跟Markov定位相比，恰恰与 ricky所说相反，粒子滤波计算量小很多，而事实上，粒子滤波被用于定位的背景就是为了降低普通的Markov定位计算量相当大并且随着维数的增长计算量迅速增长的缺陷。（Sebastian Thrun, Wolfram burgard, Dieter fox等在90年代做的一个图书馆机器人导航的项目，其中很多当时他们的工作都成了现今机器人研究领域的热点，比如粒子滤波，SLAM等）。

大家可能有几个疑问，

1. Kalman滤波或者EKF都可以做定位并且运算量小，为什么还要用什么Markov定位呢？

2. 为什么Markov定位计算量大并且随着空间维数的增长而增长剧烈？

3.为什么粒子滤波这么神奇，让计算量如此之大的Markov定位运算量骤降？

4.到底粒子滤波实质是什么？

好，现在就一一谈一下我的看法，

1. Kalman滤波或者EKF都可以做定位并且运算量小，为什么还要用什么Markov定位呢？

燢alman滤波是有适用条件的，a。初始状态必须是符合正态分布。b。必须是线性系统。（当然，EKF通过将非线性系统线性化的方法处理非线性系统）。而真实的定位问题很多时候不满足以上两个条件。为什么不满足呢？

先说为什么a不满足：首先举个正态分布无法描述的反例，大家都知道，正态分布是单峰函数，也就是说机器人初始时位于工作空间中某个位置的初始概率最大，其他地方都比这小。如果是采用地图匹配进行绝对定位，上面描述的单峰高斯函数可能就无法精确的描述事实了，比如有十个一模一样的房间。初始时把机器人放在其中一个里面，机器人根据绝对测量传感器获得局部地图，与他携带的先验地图匹配后他发现，他现在呆的位置在他的工作空间中有10个峰值，每个房间一个，因为十个房间一模一样，他无法区分。显然，此时a假设不成立。

再说b为什么不满足：这取决于真实机器人的物理特性，系统的状态更新方程是由里程计或者是dead reckon 得到的，系统的观测方程是由绝对定位系统（或者地图匹配）得到的。对于一般的移动机器人，无论是两个主动轮的形式还是一个主动轮一个steering wheel的形式，由此得到的状态更新方程都是非线性的。

2. 为什么Markov定位计算量大并且随着空间维数的增长而增长剧烈？

Markov定位的基本原理很简单，就是用条件概率描述状态更新，所有的可能的状态都枚举个遍，对于机器人的每一次更新，所有的可能的状态迁移都要被更新一遍，假设我们用栅格描述工作空间，假设t时刻机器人可能的位置为p1,p2,p3，在二维情况下采用正方形栅格划分那么p1有8个邻居，记为 p11，p12，p13，...,p18.在三维情况下，采用立方体划分那么邻居就更多了，有26个。如果维数继续增加，那么邻居增加的更厉害。这里我们以二维情况为例来阐述问题。同理，我们记p2的邻居为p21，p22，。。。p28。p3的邻居为p31，p32，。。。，p38。在t时刻可能的位置只有3个，然而t+1时刻，所有的三个的邻居，以及p1，p2，p3都有可能成为当前位置，但是根据dead reckon的结果，我们可以排除一些小概率的邻居，减少计算量。但是随着时间的推移，整个空间中的所有点都有可能成为估计的当前位置（只不过各个位置的概率不同而已）。这样，如果不采取措施，那状态的更新会是一件巨大的工程。并且，空间维数越大，节点数越多，计算量增长越厉害。

3.为什么粒子滤波这么神奇，让计算量如此之大的Markov定位运算量骤降？

粒子滤波强就强在它用统计的基于采样的方法来描述整个空间中的概率分布。Markov的思想是你既然当前位置的分布概率是个特殊分布，我就干脆把你的样本空间离散化（把空间划分为栅格），计算每一个样本的概率（计算每一个栅格的概率）。但是这带来了问题2.为了解决这个问题，粒子滤波采用了另一种思想：现在我不再均匀的把样本空间离散化了，而是根据我当前所掌握的概率分布对空间进行采样（重要性采样），显然，概率小的地方少采几个样(反正概率小，即使采多了，每个样本差别也不大，完全可以由附近的其他样本反映）；概率高的地方应该多采几个样。这样，我们可以规定，每次都采样N个，对于大概率的地方多采，小概率的地方少采。根据概率里的大数定律，可以证明即使在维数增加的时候依然保持采N个样，仍然可以保持性能。这就是粒子滤波高的地方，当维数非常高的情况，Markov定位都累的算不出来了，因为需要更新的状态对实在是太多了，而人家粒子滤波依然只采N个样，计算量还那样，变化不大。

4.到底粒子滤波实质是什么？

事实上，我们完全可以换一种思维来认识粒子滤波。就是基于奖励惩罚机制（强化学习）的优化的思想。

首先，根据状态转移方程，对于每个粒子的位置进行更新。但是这个更新只是基于dead reckon得到的，我们要融合绝对定位与相对定位，绝对定位的信息还没有融合进去呢。根据状态转移方程得到的新状态到底行不行？能有多大的概率？这还取决于绝对定位的结果也就是输出方程。把状态转移方程的到的结果代入输出方程，得到一个输出，这个输出是估计值，而根据绝对定位的观测，这个值对应的观测值也是可以测量得到的，现在这两个值之间有个差额，很明显，这个差额越小，刚才的到的状态越可信，这个差额越大，状态越不可信。好，就把这个指标作为评估函数（像GA，pso等优化算法里的evaluation function),来修正各个状态的估计概率。

总结一下，粒子滤波就是一种基于动态系统前向模型利用奖惩机制估计状态值的一种方法。

啰嗦了半天，希望对大家有帮助。

但是，如果跟的是绝对的路径，则不会出现这种情况。

回想起来上次好像也碰过这种情况，不过是一年以前的事情了，这次又折腾了半天才想起来搞明白是怎么回事。

记着，省着下次再碰见不知所措吧～

以及

再补充一个Demo of D Fusion：

国内上不了的同仁们，请翻墙上

http://www.youtube.com/watch?v=oiqIPXnKkKo

http://www.youtube.com/watch?v=TW6_X9qBeds&feature=related

http://www.youtube.com/watch?v=6NKT6eUGJDE

毕竟新媒体和人机交互是未来的趋势啊

这个是基于OpenCV和OpenGL的，算法是Optical Flow。

这个立方体的贴图有些问题，就凑合着看吧

至于我的背景和本人的穿着，我保持沉默

补充：昨天晚上睡觉前临时把贴图和运动方程做了点小的修改，现在看上去好多了～