amcl 似然域概率模型

1.首先定义一些变量

  AMCLLaser *self;
  int i, j, step;
  double z, pz;
  double log_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.确定步长，根据激光的总个数与最大波束计算

  step = ceil((data->range_count) / static_cast<double>(self->max_beams));   // 总角度/间隔大小
  
  // Step size must be at least 1
  if(step < 1)
    step = 1;

3.高斯误差  、随机偶然误差   还有最远点的概率

  // Pre-compute a couple of things
  double z_hit_denom = 2 * self->sigma_hit * self->sigma_hit;        // 高斯分布测量，sigma为标准偏差
  double z_rand_mult = 1.0/data->range_max;    // 随机测量相关，1/雷达最大距离

  double max_dist_prob = exp(-(self->map->max_occ_dist * self->map->max_occ_dist) / z_hit_denom);  //高斯指数部分

4.光束跳跃参数，

  bool do_beamskip = self->do_beamskip;
  double beam_skip_distance = self->beam_skip_distance;
  double beam_skip_threshold = self->beam_skip_threshold;
  
  //we only do beam skipping if the filter has converged    //收敛
  if(do_beamskip && !set->converged){
    do_beamskip = false;
  }

5.与地图相符的点的数量，并进行整合计算

  //我们需要计算光束与地图一致的粒子数
  int *obs_count = new int[self->max_beams]();

   //选择哪些激光点进行所有粒子的计算
  bool *obs_mask = new bool[self->max_beams]();

6.光束进行跳变的判断

  //判断是否进行光束跳跃
  bool realloc = false; 

7.当有临时跳变时，创建临时数据

 if(do_beamskip){
    if(self->max_obs < self->max_beams){            //？ 有不正常激光点
      realloc = true;
    }

    if(self->max_samples < set->sample_count){     //？粒子数目条件
      realloc = true;
    }

    if(realloc){
      self->reallocTempData(set->sample_count, self->max_beams);        //建立临时数据
      fprintf(stderr, "Reallocing temp weights %d - %d\n", self->max_samples, self->max_obs);
    }
  }

8.开始处理每一个粒子

9.对于每个粒子，遍历其激光点数，根据间隔点选取，

    for (i = 0; i < data->range_count; i += step, beam_ind++)    //遍历激光点数，间隔选点，因为计算时间原因
    {
      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;

10.将激光点映射到地图中

      // Compute the endpoint of the beam                         //
      hit.v[0] = pose.v[0] + obs_range * cos(pose.v[2] + obs_bearing);
      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]);       //将点编入格栅地图中
      mj = MAP_GYWY(self->map, hit.v[1]);

11.当该点位于边界时，看做最大障碍物距离，否则取最小障碍物距离，当最小距离小于设定的波束跳跃距离时，可用点+1，同时确定高斯噪声。

      if(!MAP_VALID(self->map, mi, mj)){      //地图外看作最大障碍物距离
        pz += self->z_hit * max_dist_prob;
      }
      else{
        z = self->map->cells[MAP_INDEX(self->map,mi,mj)].occ_dist;  //最小障碍物距离
        if(z < beam_skip_distance){
           obs_count[beam_ind] += 1;
      }
        pz += self->z_hit * exp(-(z * z) / z_hit_denom);      //高斯模型
      }

12.在加上随机模型，检测概率

      // Gaussian model
      // NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma)
      
      // Part 2: random measurements
      pz += self->z_rand * z_rand_mult;               //随机测量模型

      assert(pz <= 1.0); 
      assert(pz >= 0.0);

13.做不做跳变进行独立处理，不做跳变时权重正常相加

      if(!do_beamskip){
        log_p += log(pz);
      }
      else{
        self->temp_obs[j][beam_ind] = pz;         //短读数处理
      }

   if(!do_beamskip){
      sample->weight *= exp(log_p);
      total_weight += sample->weight;
    }

14.做跳变时，如果跳变的个数占总个数一定权重时，设定障碍物遮挡

   int skipped_beam_count = 0; 
    for (beam_ind = 0; beam_ind < self->max_beams; beam_ind++){
      if((obs_count[beam_ind] / static_cast<double>(set->sample_count)) > beam_skip_threshold){
          obs_mask[beam_ind] = true;
      }
      else{
          obs_mask[beam_ind] = false;
          skipped_beam_count++;
      }
    }

15.如果跳过光束大于跳过错误的阈值时，erraor = true

  if(skipped_beam_count >= (beam_ind * self->beam_skip_error_threshold)){
      fprintf(stderr, "Over %f%% of the observations were not in the map - pf may have converged to wrong pose - integrating all observations\n", (100 * self->beam_skip_error_threshold));
      error = true; 
    }

16.

    for (j = 0; j < set->sample_count; j++)
    {
        sample = set->samples + j;
        pose = sample->pose;

        log_p = 0;

        for (beam_ind = 0; beam_ind < self->max_beams; beam_ind++){
           if(error || obs_mask[beam_ind]){
              log_p += log(self->temp_obs[j][beam_ind]);
          }
        }

      sample->weight *= exp(log_p);

      total_weight += sample->weight;
    }