amcl laser_sensor

1.通过特定的感知模型进行粒子滤波器的更新

bool AMCLLaser::UpdateSensor(pf_t *pf, AMCLSensorData *data)

bool AMCLLaser::UpdateSensor(pf_t *pf, AMCLSensorData *data)      //选择模型
{
  if (this->max_beams < 2)
    return false;

  // Apply the laser sensor model
  if(this->model_type == LASER_MODEL_BEAM)
    pf_update_sensor(pf, (pf_sensor_model_fn_t) BeamModel, data);
  else if(this->model_type == LASER_MODEL_LIKELIHOOD_FIELD)
    pf_update_sensor(pf, (pf_sensor_model_fn_t) LikelihoodFieldModel, data);  
  else if(this->model_type == LASER_MODEL_LIKELIHOOD_FIELD_PROB)
    pf_update_sensor(pf, (pf_sensor_model_fn_t) LikelihoodFieldModelProb, data);  
  else
    pf_update_sensor(pf, (pf_sensor_model_fn_t) BeamModel, data);

  return true;
}

根据传感器的模型选择合理的模型预计，本章主要介绍似然域模型

pf_update_sensor(pf, (pf_sensor_model_fn_t) LikelihoodFieldModel, data);

其中分为两个函数，1.似然域模型，2.粒子跟新

1.似然域模型

遍历整个粒子集合，重新计算权重

1.首先定义了一些相关的数据

  AMCLLaser *self;
  int i, j, step;
  double z, pz;
  double p;
  double obs_range, obs_bearing;
  double total_weight;
  pf_sample_t *sample;
  pf_vector_t pose;
  pf_vector_t hit;

  self = (AMCLLaser*) data->sensor;

  total_weight = 0.0;

2.遍历每一个粒子

  for (j = 0; j < set->sample_count; j++)          //遍历每个粒子

3.这个函数是将传感器局部坐标经过三角变换映射到全局坐标系下

 pose = pf_vector_coord_add(self->laser_pose, pose);     //转换坐标系，与激光雷达数据匹配

4.预计算似然域，离散栅格化

    // Pre-compute a couple of things
    double z_hit_denom = 2 * self->sigma_hit * self->sigma_hit;   //测量噪声的方差       //P130  8
    double z_rand_mult = 1.0/data->range_max;               //无法解释的随机测量的分布     //P130 8

5.因为时间的问题，这不是全部点都进行似然计算，这里只是间隔地选点，

    //因为时间的问题，这不是全部点都进行似然计算，这里只是间隔地选点，
    step = (data->range_count - 1) / (self->max_beams - 1); //计算步长，间隔选取激光点

    // Step size must be at least 1
    if(step < 1)
      step = 1;

6.开始通过利用与最近物体的欧氏距离计算激光模型似然的算法，对所有特征（激光数据）进行遍历

for (i = 0; i < data->range_count; i += step)       //间隔遍历激光点

7.将激光数据转化为栅格地图，进行离散话，

似然域测量模型简单地将 最大距离、nan  读数丢弃

      obs_range = data->ranges[i][0];                    //距离
      obs_bearing = data->ranges[i][1];                  //角度

      // This model ignores max range readings      //距离大于最大距离的点，丢弃
      if(obs_range >= data->range_max)
        continue;

      // Check for NaN
      if(obs_range != obs_range)
        continue;

      pz = 0.0;

      //激光点通过两次转化到全局坐标系，  map  base_link  laser
      // Compute the endpoint of the beam                                    //hit高斯
      hit.v[0] = pose.v[0] + obs_range * cos(pose.v[2] + obs_bearing);       //激光点在全局坐标系下，pose ：雷达在全局坐标
      hit.v[1] = pose.v[1] + obs_range * sin(pose.v[2] + obs_bearing);
      // Convert to map grid coords.       转化为栅格坐标
      int mi, mj;
      mi = MAP_GXWX(self->map, hit.v[0]);
      //计算函数在map_cspace.cpp的map_update_cspace中实现遍历计算，该函数是被AMCLLaser::SetModelLikelihoodField调用
      mj = MAP_GYWY(self->map, hit.v[1]);            //距离和角度格栅

8.得到该点到障碍物的最近距离

这一块的参数在map_update_cspace函数中计算，具体来说是将整个地图代入并计算障碍物位置，并将最近距离预先计算好，再把当前的栅格代入即可

    // Part 1: Get distance from the hit to closest obstacle.      //这是提前计算好的最近距离，
      // Off-map penalized as max distance
      if(!MAP_VALID(self->map, mi, mj))
        z = self->map->max_occ_dist;                             //不在地图范围内的当成最大距离
      else
        z = self->map->cells[MAP_INDEX(self->map,mi,mj)].occ_dist;     
       //计算距离地图障碍物的最小值，即找到激光终点对应的障碍物点

9.计算zt似然域的算法，在障碍物点周围用正态分布和均匀分布计算似然域，是关于dist的函数

      // NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma)
      pz += self->z_hit * exp(-(z * z) / z_hit_denom);     //高斯测量  
      pz += self->z_rand * z_rand_mult;                //随机测量 ，两个测量模型

10.检测，并将每个激光点计算的似然域累加起来

      assert(pz <= 1.0);
      assert(pz >= 0.0);
      //      p *= pz;
      // here we have an ad-hoc weighting scheme for combining beam probs
      // works well, though...
      p += pz*pz*pz;                               //将每个激光点计算的似然域累加起来（多大范围）

11.得到单个粒子的权重并加入到粒子集中

  sample->weight *= p;                     //概率等于权重
    total_weight += sample->weight;
  }    //粒子循环

12.返回粒子的总权重  return(total_weight);

2.粒子的跟新

实现对所有粒子更新权重，并归一化、计算出《概率机器人》8.3.5的失效恢复的长期似然和短期似然

1. 定义了粒子集、单个粒子

  int i;
  pf_sample_set_t *set;      //粒子集合
  pf_sample_t *sample;       //单个粒子
  double total;                              //pf 滤波器

  set = pf->sets + pf->current_set;     //指针，指向sets里的序号

2.计算所有粒子粒子的总权重

  // Compute the sample weights
  total = (*sensor_fn) (sensor_data, set);    //总权重

3.当粒子的总权重为零时，样本的权重为 1/样本的个数

将各粒子集的权值归一化

    // Handle zero total
    for (i = 0; i < set->sample_count; i++)
    {
      sample = set->samples + i;
      sample->weight = 1.0 / set->sample_count;
    }

4.否则，计算粒子的平均权重，并且每个粒子除以粒子群权重

    // Normalize weights
    double w_avg=0.0;
    for (i = 0; i < set->sample_count; i++)
    {
      sample = set->samples + i;
      w_avg += sample->weight;                 //所有粒子权重加起来
      sample->weight /= total;                 //total？
    }
    // Update running averages of likelihood of samples (Prob Rob p258)
    w_avg /= set->sample_count;               //计算平均权重

5.更新短期似然平均与长期似然平均

    if(pf->w_slow == 0.0)
      pf->w_slow = w_avg;
    else
      pf->w_slow += pf->alpha_slow * (w_avg - pf->w_slow);
    if(pf->w_fast == 0.0)
      pf->w_fast = w_avg;
    else
      pf->w_fast += pf->alpha_fast * (w_avg - pf->w_fast);
    //计算长期短期似然：衰减率不一样alpha-slow《《alpha-fast
    //printf("w_avg: %e slow: %e fast: %e\n", 
           //w_avg, pf->w_slow, pf->w_fast);

2.重采样

1. 重采样的方法比较经典，根据w值增加了一部分随机生成的粒子，当drand48<w_diff，随机均匀在地图范围内产生一个粒子。这里是调用uniformPoseGenerator产生的。
2. drand48>w_diff时，将PDF积分得到CDF，然后drand48来对应离散CDF中采样，这样drand48落在哪个点上就把哪个粒子复制一份，将所有粒子权重置为1，完成重采样。然后对每个粒子权重进行归一化，重置fast和slow，重新计算统计数据（均值，方差）
    

在各粒子集的权值更新后便可进行重采样，重采样中淘汰一些权值较低的粒子，并加入一些粒子。这些新加入的粒子可以是随机加入的，也可以是根据当前观测值加权后的粒子。这样便可以将新的小箭头们发布了，也可以发布新估算出的权值最大的粒子集的平均位姿。

void pf_update_resample(pf_t *pf)

1.定义一些参数，确定样本set_a、set_b的变化

 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;       //%2 ?

2.建立重采样的累积概率表--用更有效的程序替换它

  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;           //权重累积

 3.为自适应采样创建kd树

  pf_kdtree_clear(set_b->kdtree);

4. 从粒子集a生成粒子集b

  w_diff = 1.0 - pf->w_fast / pf->w_slow;
  if(w_diff < 0.0)
    w_diff = 0.0;                                 //p195，8.5

5.不能容易地将低方差采样器与KLD自适应组合采样，所以我们采取更传统的路线。

  while(set_b->sample_count < pf->max_samples)

6.随机增加分布的粒子

    if(drand48() < w_diff)     
   //0-1之间均匀分布的随机数，按照概率增加，w_diff越大，增加粒子的可能性也越大
      sample_b->pose = (pf->random_pose_fn)(pf->random_pose_data);   //增加随机分布粒子，重采样

不能将低方差与KLD自适应，需重采样

8.随机生成一个数，用于重采样，即:概率低的粒子直接break掉

      // 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;
      }

9.生成随机数位置增加一个粒子

      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;    
      //在a即生成随机数位置增加一个粒子，某个位置附近粒子数越多或者权重越大，这个位置生成重采样的粒子概率越大

10.将粒子标准话

    sample_b->weight = 1.0;                //每个粒子都是1，之后标准化
    total += sample_b->weight;             //total总粒子数 or 总粒子权重，total值？

    // Add sample to histogram
    pf_kdtree_insert(set_b->kdtree, sample_b->pose, sample_b->weight);      //将样本添加到直方图，关于位姿的二叉树

11.重置平均值，以避免螺旋式变为完全随机性。

  // 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
  }
  

12.  重新计算群集统计信息

  // 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);