xk = fk(xk-1,vk), 如 xk = xk-1 / 2 + 25 * xk-1 / ( 1 + xk-1 * xk-1) + 8 * cos(1.2*(k-1)) + vk 
  yk = hk(xk, nk),  如 yk = xk * xk / 20 + nk 

  x 为 系统的状态， y为对系统状态x的测量值，
  f是系统状态转移函数，h为系统测量函数，
  v是系统过程噪声， n是系统测量噪声

  从贝叶斯理论的观点来看，状态估计问题（目标跟踪、信号滤波）
  就是根据之前一系列的已有数据 y1：yk（后验知识）递推的计算出当前状态 xk的可信度，
  这个可信度就是概率公式 
  p(xk|y1:yk)，
  它需要通过预测和更新两个步奏来递推的计算。 ### 11.1.2 预测过程  先验概率
  通过已有的先验知识对未来的状态进行猜测，即
  先验概率  p( x(k)|x(k-1) )

  利用最新的测量值对先验概率密度进行修正，
  得到后验概率密度，也就是对之前的猜测进行修正。 ### 11.1.4 假设系统的状态转移服从一阶马尔科夫模型
  当前时刻的状态x(k)只与上一个时刻的状态x(k-1)有关
  例如 掷筛子，前进几步
  
  同时，假设k时刻测量到的数据y(k)只与当前的状态x(k)有关， ### 11.1.5 推导
  已知：
        已知k-1时刻的概率密度函数 , p(xk-1|y1:yk-1)
  预测：
       p(xk|y1:yk-1) = 积分(p(xk,xk-1|y1:yk-1)*dxk-1)
                     = 积分( p(xk|xk-1,y1:yk-1) * p(xk-1|y1:yk-1) * dxk-1 )
                       (一阶马尔科夫过程的假设，状态x(k)只由x(k-1)决定)
                     = 积分( p(xk|xk-1) * p(xk-1|y1:yk-1) * dxk-1 )
       要采样x(k)，直接采样一个过程噪声，再叠加上 f(x(k-1)) 这个常数就行了。
       
  更新：
      p(xk|y1:yk) = p(yk|xk,y1:yk-1) * p(xk|y1:yk-1)/ p(yk|y1:yk-1)

  蒙特卡洛采样的思想就是用平均值来代替积分
  抛硬币的例子一样，抛的次数足够多就可以用来估计正面朝上或反面朝上的概率了。
  
  其实就是想知道当前状态的期望值：
  E(f) = 1/N * sum(f(xi)) 
  就是用这些采样的粒子的状态值直接平均就得到了期望值，
  也就是滤波后的值，这里的 f(x) 就是每个粒子的状态函数。
  这就是粒子滤波了，只要从后验概率中采样很多粒子，用它们的状态求平均就得到了滤波结果。
  
   思路看似简单，但是要命的是，后验概率不知道啊，怎么从后验概率分布中采样！
   所以这样直接去应用是行不通的，这时候得引入重要性采样这个方法来解决这个问题。

  无法从目标分布中采样，
  就从一个已知的可以采样的分布里去采样如 q(x|y)，
  这样上面的求期望问题就变成了：  
  E(f) = Wi‘ * sum(f(xi)) 
  Wi’ =  Wi/sum(Wi) 归一化的权重
  而是一种加权和的形式。
  不同的粒子都有它们相应的权重，如果粒子权重大，说明信任该粒子比较多。

  到这里已经解决了不能从后验概率直接采样的问题，
  但是上面这种每个粒子的权重都直接计算的方法，效率低。

  最佳的形式是能够以递推的方式去计算权重，这就是所谓的序贯重要性采样（SIS），粒子滤波的原型。

  1、 采样
  2、 递推计算各粒子权重
  3、 粒子权值归一化
  4、 对每个粒子的状态进行加权去估计目标的状态了

  在应用SIS 滤波的过程中，存在一个退化的问题。
  就是经过几次迭代以后，很多粒子的权重都变得很小，可以忽略了，只有少数粒子的权重比较大。
  并且粒子权值的方差随着时间增大，状态空间中的有效粒子数较少。
  随着无效采样粒子数目的增加，
  使得大量的计算浪费在对估计后验滤波概率分布几乎不起作用的粒子上，使得估计性能下降，
  克服序贯重要性采样算法权值退化现象最直接的方法是增加粒子数，
  而这会造成计算量的相应增加，影响计算的实时性。
  因此，一般采用以下两种途径：
       1)选择合适的重要性概率密度函数；
       2)在序贯重要性采样之后，采用重采样方法。
       
  重采样的思路是：
      既然那些权重小的不起作用了，那就不要了。
      要保持粒子数目不变，得用一些新的粒子来取代它们。
      找新粒子最简单的方法就是将权重大的粒子多复制几个出来，至于复制几个？
      那就在权重大的粒子里面让它们根据自己权重所占的比例去分配，
      也就是老大分身分得最多，
      老二分得次多，以此类推。
      
      类似遗传算法的 淘汰 和 变异产生新个体
      
  假设有3个粒子，在第k时刻的时候，他们的权重分别是0.1, 0.1 ,0.8, 
  然后计算他们的概率累计和(matlab 中为cumsum() )得到： [0.1, 0.2, 1]。
  接着，我们用服从[0,1]之间的均匀分布随机采样3个值，
  假设为0.15 , 0.38 和 0.54。
  也就是说，第二个粒子复制一次，第三个粒子复制两次。 

  1、 粒子采样初始化，均匀分布采样/高斯分布采样
  2、 重要性采样，递推计算各粒子权重，并归一化 粒子权值
  3、 对权重值较小的粒子进行更新，重采样，用权重大的复制替换，(可能会逐渐丢失个体特性，可尝试使用正则粒子滤波，遗传算法，会使用交叉变异)
  4、 对每个粒子的状态进行加权去估计目标的状态 

  通俗解释：
  1、初始化阶段——计算目标特征，比如人体跟踪，就是人体区域的颜色直方图等，n*1的向量；
  2、搜索化阶段——放警犬(采样大量粒子，用来发现目标)，可以 均匀的放置/按目标中心高斯分布放置；
              ——狗鼻子发现目标，按照相似度信息，计算警犬距离目标的权重，并归一化；
  3、决策化阶段——收集信息，综合信息，每条小狗有一个位置信息和一个权重信息，我们进行 加权求和得到目标坐标；
  4、重采样阶段——去掉一些跑偏的警犬，再放入一些警犬，根据权重信息，将权重低的警犬调回来，重新放置在权重高的地方；
                 根据重要性重新放狗 
  2-> 3-> 4-> 2

  思考1：

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

  思考2：
  感觉粒子滤波和遗传算法真是像极了。
  同时，如果你觉得这种用很多粒子来计算的方式效率低，
  在工程应用中不好接受，推荐看看无味卡尔曼滤波（UKF）,
  他是有选择的产生粒子，而不是盲目的随机产生。

  %% SIR粒子滤波的应用，算法流程参见博客http://blog.csdn.net/heyijia0327/article/details/40899819  
  clear all  
  close all  
  clc  
  %% initialize the variables  
  x = 0.1; % 系统初始状态 initial actual state  
  x_N = 1; % 系统过程噪声的协方差  (由于是一维的，这里就是方差)    vk
  x_R = 1; % 测量的协方差                                       nk
  T = 75;  % 共进行75次  
  N = 100; % 粒子数，越大效果越好，计算量也越大  

  %initilize our initial, prior particle distribution as a gaussian around  
  %the true initial value  

  V = 2;    % 初始分布的方差  
  x_P = []; % 粒子  100个
  % 用一个高斯分布随机的产生初始的粒子  
  for i = 1:N  
      x_P(i) = x + sqrt(V) * randn; % 生成初始值 
  end  
   %% xk = fk(xk-1,vk), 如 xk = xk-1 / 2 + 25 * xk-1 / ( 1 + xk-1 * xk-1) + 8 * cos(1.2*(k-1)) + vk 
   %% yk = hk(xk, nk),  如 yk = xk * xk / 20 + nk  测量值
  z_out = [x^2 / 20 + sqrt(x_R) * randn];  % 实际测量值  
  x_out = [x];  % 系统the actual output vector for measurement values.  
  x_est = [x];  % 状态估计值time by time output of the particle filters estimate  
  x_est_out = [x_est]; % the vector of particle filter estimates.  

  for t = 1:T %迭代次数 
      x = 0.5*x + 25*x/(1 + x^2) + 8*cos(1.2*(t-1)) +  sqrt(x_N)*randn; % 系统状态 传递
      z = x^2/20 + sqrt(x_R)*randn;  % 测量值
      for i = 1:N %计算 每一个粒子的权重
            % 从先验p(x(k)|x(k-1))中采样  
          x_P_update(i) = 0.5*x_P(i) + 25*x_P(i)/(1 + x_P(i)^2) + 8*cos(1.2*(t-1)) + sqrt(x_N)*randn;  
            % 计算采样粒子的值，为后面根据似然去计算权重做铺垫  
          z_update(i) = x_P_update(i)^2/20;% 采样粒子的 测量值  
            % 对每个粒子计算其权重，这里假设量测噪声是高斯分布。所以 w = p(y|x)对应下面的计算公式  
          P_w(i) = (1/sqrt(2*pi*x_R)) * exp(-(z - z_update(i))^2/(2*x_R));  
      end  
      % 归一化.  
      P_w = P_w./sum(P_w);  

      %% Resampling这里没有用博客里之前说的histc函数，不过目的和效果是一样的  
      for i = 1 : N  
          x_P(i) = x_P_update(find(rand <= cumsum(P_w),1));   % 粒子权重大的将多得到后代  
      end                                                     % find( ,1) 返回第一个 符合前面条件的数的 下标  

      %状态估计，重采样以后，每个粒子的权重都变成了1/N  
      x_est = mean(x_P); % 均值为估计值 

      % Save data in arrays for later plotting  
      x_out = [x_out x]; % 系统状态 
      z_out = [z_out z]; % 测量值 
      x_est_out = [x_est_out x_est];  % 系统粒子滤波估计值
  end  

  t = 0:T;  
  figure(1);  
  clf  
  plot(t, x_out, '.-b', t, x_est_out, '-.r','linewidth',3);  
  set(gca,'FontSize',12); set(gcf,'Color','White');  
  xlabel('time step'); ylabel('flight position');  
  legend('True flight position', 'Particle filter estimate');