粒子滤波重采样的理解及MATLAB和C++实现

0. 序贯重要性重采样SIR(粒子滤波)

  序贯重要性采样(SIS)是对重要性采样的一种序列形式，该算法可对一般状态空间模型的滤波分布进行重要性采样。重采样主要是为了解决经典蒙特卡洛方法中出现的粒子匮乏现象。若有效粒子数较少，则需要进行重采样，避免活跃粒子过少导致陷入局部最优，同时避免将计算量消耗在权值较小的粒子上。
  其主要思想是对粒子和其相应的权值表示的概率密度函数重新进行采样。通过增加权值较大粒子和减少权值较小粒子来实现。重采样虽然可以改善粒子匮乏现象，但也降低了粒子的多样性。因此，重采样过程中一般选取一些准则来判断有效粒子的个数，通过这个个数来判断是否进行重采样。一般的判断准则为：

  其中Neff为有效粒子个数，表示粒子权值,N为粒子个数。通过将Neff与预先设定的个数进行比较来决定是否重采样。一般Neff<2/3*N时候进行重采样。
  在序贯序贯重要性采样中，存在大多是粒子的权值为零或者近似为零。这一现象称为粒子退化问题。利用重采样法可以解决粒子退化的问题。

  从先验分布中抽取$N$个样本${X_0}^{(i)}$
$${X_0}^{(i)}P(x_0),i=1,......N$$
  对所有的$i=1,......N$，令${{\omega }_0}^{(i)}=\frac{1}{N}$
  对每个$K=1,......T$，有
  1). 从重要性分布中抽取$x_k^{(}i)$

  2). 计算权值

将权值归一化，使其和为1。
  3). 估计有效粒子数，$n_{eff}$，若有效粒子数过少，则执行重采样。

  该算法的性能2依赖于重要性分布π ( ⋅ ) \bm{\pi(\cdot)}π(⋅)的选择。重要性分布应满足易于从重要性分布中抽取样本，且能够计算样本点的概率密度为原则。方差最优重要性分布为；

  如果最优重要性分布不能直接实现，则可通过进行局部线性化得到。利用扩展卡尔曼滤波EKF，无迹卡尔曼滤波UKF等非线性卡尔曼滤波器，可得到重要性分布。

1.重采样过程

从离散分布中抽取N个样本来带替旧的样本集。具体参考下链接：
https://www.cnblogs.com/gary-guo/p/6244011.html

2. 粒子滤波重采样常用方法

①轮盘赌②低方差采样

2.1 轮盘赌

2.1.1 重采样具体过程伪代码形式如下：

按照如此方法，权值比较大的粒子会被多次复制，那些权值小对计算后验概率函数贡献非常小的粒子就会被剔除。上面的过程其实就是相当于一个转转盘的过程，如下图所示：

这个转盘被分成了N分，即从w1到wn，和为1。每个区间就是一个粒子的权值，权值越大，区间的弧度就越大。每次都从w1左边的竖线开始转，显而易见，转到w1的概率为0-w1之间的随机数，转到w2的概率为w1到w1+w2之间的随机数，转到w3的概率为w1+w2到w1+w2+w3之间的随机数，后面的依次类推。当某个粒子的权值较大的时候，产生的随机数落在相应区间的概率就会增大，从而实现了对权值大粒子的多次复制，权值小的粒子的剔除。这样重采样的过程中不是全部复制大权值粒子，也有可能对小权值粒子进行了复制，因为区间虽小，但也有可能转到这个区间。不过这样在一定程度上也保证了粒子的多样性。

2.1.2 MATLAB代码如下

function w_new=resample_particles1(w)
w_new=w;
Neff=1/sum(w.*w);
N=length(w);
if Neff<75 %75为预先设定阈值
    for i = 1 : N
        u = rand; 
        qtempsum = 0;
        for j = 1 : N
            qtempsum = qtempsum + w(j);
            if qtempsum >= u
                w_new(i)=w(j);
                break;
            end
        end
    end
end

2.1.3 不足及应对方法

重采样使得粒子的多样性降低。这将加大采样误差。因此在实际使用中要制定合理策略，控制采样条件，例如状态静止时停止采样等。总结来说，当权重偏向大数量样本时，可以降低采样频率，以减小状态多样性的丢失。当权重偏向小数量样本时，应该进行重采样。

2.1.4 C++如下

// Resample the distribution
void pf_update_resample(pf_t *pf)
{
  int i;
  double total;
  pf_sample_set_t *set_a, *set_b;
  pf_sample_t *sample_a, *sample_b;
 
  //double r,c,U;
  //int m;
  //double count_inv;
  double* c;
 
  double w_diff;
 
  set_a = pf->sets + pf->current_set;//current，只有0和1，a和b交替指向sets的第一个和第二个位置，
                                     //每个周期由a生成b，但是a所指向的sets的位置不一样，a在本个周期指向的是上个周期中b的位置
  set_b = pf->sets + (pf->current_set + 1) % 2;
  //set_a为上周期采样的粒子，set_b为本周期将要采样的粒子
 
 
  // Build up cumulative probability table for resampling.
  // TODO: Replace this with a more efficient procedure
  // (e.g., http://www.network-theory.co.uk/docs/gslref/GeneralDiscreteDistributions.html)
  c = (double*)malloc(sizeof(double)*(set_a->sample_count+1));//分配count+1个 double类型的储存单元
  c[0] = 0.0;
  for(i=0;i<set_a->sample_count;i++)//权重累积
    c[i+1] = c[i]+set_a->samples[i].weight;
 
  // Create the kd tree for adaptive sampling
  pf_kdtree_clear(set_b->kdtree);//为自适应采样创建kd树
  
  // Draw samples from set a to create set b.
  total = 0;
  set_b->sample_count = 0;
 
  w_diff = 1.0 - pf->w_fast / pf->w_slow;
  if(w_diff < 0.0)
    w_diff = 0.0;
//  printf("w_diff: %9.6f\n", w_diff);
 
  // Can't (easily) combine low-variance sampler with KLD adaptive
  // sampling, so we'll take the more traditional route.
  //不能容易地将低方差采样器与KLD自适应组合采样，所以我们采取更传统的路线。
  /*
  // Low-variance resampler, taken from Probabilistic Robotics, p110
  count_inv = 1.0/set_a->sample_count;
  r = drand48() * count_inv;
  c = set_a->samples[0].weight;
  i = 0;
  m = 0;
  */
 
  while(set_b->sample_count < pf->max_samples)
  {
    sample_b = set_b->samples + set_b->sample_count++;
    //0-1之间均匀分布的随机数，按照概率增加，w_diff越大，增加粒子的可能性也越大
    if(drand48() < w_diff)
      sample_b->pose = (pf->random_pose_fn)(pf->random_pose_data);//增加随机分布粒子，重采样
    else
    {
      // Can't (easily) combine low-variance sampler with KLD adaptive
      // sampling, so we'll take the more traditional route.
      /*
      // Low-variance resampler, taken from Probabilistic Robotics, p110
      U = r + m * count_inv;
      while(U>c)
      {
        i++;
        // Handle wrap-around by resetting counters and picking a new random
        // number
        if(i >= set_a->sample_count)
        {
          r = drand48() * count_inv;
          c = set_a->samples[0].weight;
          i = 0;
          m = 0;
          U = r + m * count_inv;
          continue;
        }
        c += set_a->samples[i].weight;
      }
      m++;
      */
 
      // Naive discrete event sampler  离散采样器
      double r;
      r = drand48();//生成一个随机数
 
      for(i=0;i<set_a->sample_count;i++)
      {
        if((c[i] <= r) && (r < c[i+1]))//将随机数以权重为概率分配到某处
          break;
      }
      assert(i<set_a->sample_count);
 
      sample_a = set_a->samples + i;
 
      assert(sample_a->weight > 0);
 
      // Add sample to list
      sample_b->pose = sample_a->pose;
      //在一个即生成随机数位置增加一个粒子，某个位置附近粒子数越多或者权重越大，这个位置生成重采样的粒子概率越大
    }
 
    sample_b->weight = 1.0;//每个粒子都是1，之后标准化
    total += sample_b->weight;
 
    // Add sample to histogram
    pf_kdtree_insert(set_b->kdtree, sample_b->pose, sample_b->weight);//将样本添加到直方图，关于位姿的二叉树
 
    // See if we have enough samples yet
    if (set_b->sample_count > pf_resample_limit(pf, set_b->kdtree->leaf_count))
      break;
  }
  
  // Reset averages, to avoid spiraling off into complete randomness.
  if(w_diff > 0.0)
    pf->w_slow = pf->w_fast = 0.0;//增加粒子集后重置似然
 
  //fprintf(stderr, "\n\n");
 
  // Normalize weights
  for (i = 0; i < set_b->sample_count; i++)
  {
    sample_b = set_b->samples + i;
    sample_b->weight /= total; //重采样以后每个粒子权重=1 / M
  }
  
  // Re-compute cluster statistics 聚类，得到均值和方差等信息，将相近的一堆粒子融合成一个粒子
  pf_cluster_stats(pf, set_b);
 
  // Use the newly created sample set  //新粒子集
  pf->current_set = (pf->current_set + 1) % 2; //current 0和1交替
 
  pf_update_converged(pf);//计算滤波器是否收敛
 
  free(c);
  return;
}

3. 低方差采样