粒子滤波重采样

重采样主要是为了解决经典蒙特卡洛方法中出现的粒子匮乏现象。其主要思想是对粒子和其相应的权值表示的概率密度函数重新进行采样。通过增加权值较大粒子和减少权值较小粒子来实现。重采样虽然可以改善粒子匮乏现象，但也降低了粒子的多样性。

两种较为常用的重采样算法：轮盘赌、低方差采样。

一、轮盘赌（独立随机采样）

每个粒子对应的权重大小就是图中各奖项对应的面积大小。每次采样就是转动一次转盘。

                                              

但这种采样方式有一些缺点，我们举个例子来看。假设机器人静止，且没有观测输入，也就是机器人不知道有关其位置的任何信息，也就是其位置在下图空间中属于均匀分布，我们以下图中均匀抽样的粒子表示。

                                             

如果在此基础上采用轮盘赌采样进行粒子滤波，理想结果应该为粒子保持不动。因为机器人没有移动，且没有观测，每个粒子的权重也相等。但实际情况并非如此，采样后结果如下图所示。

                                            

重采样使得粒子的多样性降低。这将加大采样误差。

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

代码：

// 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;isample_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;isample_count;i++)
      {
        if((c[i] <= r) && (r < c[i+1]))//将随机数以权重为概率分配到某处
          break;
      }
      assert(isample_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;
}

二、低方差采样