在AMCL中，里程计是作为状态预测器存在的，通过接受当前的控制信号，从上一帧机器人状态对这一帧机器人状态进行预测，并与当前观测的结果对当前预测进行加权打分。所以通过输入和输出我们知道，里程计在AMCL中的作用就是根据当前控制信号更新上一帧的能表征机器人状态的粒子集合。

给机器人一个初始位姿，粒子才根据初始位姿进行初始化分布，才会进行更新(UpdateAction函数)，否则不会有amcl_pose话题输出，而更新依赖于读取到传感器数据发生变化，如在amcl中里程计的数据变化等，也就是机器人的运动

工具函数和类

/* 两个处理角度的函数，保证角度都在(-π，π] 的范围内 */

// 比如 30°的弧度,代入后基本还是得30°的弧度
// 但是对于210°, 代入得到的是-150°的弧度值
static double normalize(double z)
{
  return atan2(sin(z),cos(z));
}

// 对于240和30的弧度,返回-150的弧度
static double angle_diff(double a, double b)
{
  double d1, d2;
  a = normalize(a);
  b = normalize(b);
  d1 = a-b;
  d2 = 2*M_PI - fabs(d1);
  if(d1 > 0)
    d2 *= -1.0;
  if(fabs(d1) < fabs(d2))
    return(d1);
  else
    return(d2);
}

// 向量，成员为[x,y,θ]
typedef struct
{
  double v[3];
} pf_vector_t;

// Odometric sensor data
class AMCLOdomData : public AMCLSensorData
{
  // Odometric pose
  public: pf_vector_t pose;

  // Change in odometric pose
  public: pf_vector_t delta;
};

// 单个粒子的采样信息，只有位姿和权重
typedef struct
{
  // Pose represented by this sample
  pf_vector_t pose;

  // Weight for this pose
  double weight;
  
} pf_sample_t;


// 从零均值的高斯分布获得随机值，标准差为sigma
// We use the polar form of the Box-Muller transformation
double pf_ran_gaussian(double sigma)
{
  double x1, x2, w, r;
  do
  {
    do { r = drand48(); } while (r==0.0);
    x1 = 2.0 * r - 1.0;
    do { r = drand48(); } while (r==0.0);
    x2 = 2.0 * r - 1.0;
    w = x1*x1 + x2*x2;
  } while(w > 1.0 || w==0.0);
 
  return(sigma * x2 * sqrt(-2.0*log(w)/w));
}

laserReceived中的准备工作

下面来看回调函数laserReceived中的运动模型准备工作，我们需要的是odata，它的类型就是上面的AMCLOdomData，以delta成员为例，它的赋值顺序是： odata.delta <—- delta <—- pose(当前位姿)和pfodom_pose(上一次位姿)

// 获得雷达数据时刻的odom坐标系中的坐标,赋值给pose
pf_vector_t pose;
if(!getOdomPose(latest_odom_pose_, pose.v[0], pose.v[1], pose.v[2],
              laser_scan->header.stamp, base_frame_id_))
{
    ROS_ERROR("Couldn't determine robot's pose associated with laser scan");
    return;
}

pf_vector_t delta = pf_vector_zero();
// 完成初始化后执行
if(pf_init_)
{
// 位姿相对上一次的变化， pf_odom_pose_就是上一时刻的位姿
delta.v[0] = pose.v[0] - pf_odom_pose_.v[0];
delta.v[1] = pose.v[1] - pf_odom_pose_.v[1];
delta.v[2] = angle_diff(pose.v[2], pf_odom_pose_.v[2]);

// 是否更新粒子滤波器，位移需要大于参数update_min_d， 角度需要大于参数update_min_a
bool update = fabs(delta.v[0]) > d_thresh_ ||
              fabs(delta.v[1]) > d_thresh_ ||
              fabs(delta.v[2]) > a_thresh_;
update = update || m_force_update;
m_force_update=false;

// Set the laser update flags
if(update)
  for(unsigned int i=0; i < lasers_update_.size(); i++)
    lasers_update_[i] = true;
}

bool force_publication = false;
// 第一次执行laserReceived时执行, 之前的handleMapMessage里还是false
// 如果里程计的数据显示机器人已经发生了明显的位移或者旋转，标记所有的雷达更新为true

// 难道这里只在开始运行一次 ???
if(!pf_init_)
{
    // 上一次滤波器更新的位姿 
    pf_odom_pose_ = pose;

    // 滤波器现在初始化了
    pf_init_ = true;

    // Should update sensor data
    for(unsigned int i=0; i < lasers_update_.size(); i++)
      lasers_update_[i] = true;

    force_publication = true;
    resample_count_ = 0; 	// 重采样次数
}
// 刚启动amcl时不执行,机器人移动后执行, 更新滤波器
else if(pf_init_ && lasers_update_[laser_index])
{
    AMCLOdomData odata;
    odata.pose = pose;
    odata.delta = delta;

    // 上一帧位姿粒子状态pf和当前传感器信息，更新滤波器
    odom_->UpdateAction(pf_, (AMCLSensorData*)&odata);
}
bool resampled = false;

到此就是算法的准备工作了, pf_是粒子滤波器，在handleMapMessage中创建和初始化

UpdateAction

接下来就是运动模型的算法了，对应《概率机器人》103页：

// Apply the action model
bool AMCLOdom::UpdateAction(pf_t *pf, AMCLSensorData *data)
{
  AMCLOdomData *ndata;
  ndata = (AMCLOdomData*) data;

  // Compute the new sample poses
  pf_sample_set_t *set;
  // 确立当前的粒子集合，我们要产生的是一堆有位姿的粒子
  set = pf->sets + pf->current_set;
  // 上一时刻的位姿
  pf_vector_t old_pose = pf_vector_sub(ndata->pose, ndata->delta);
  // 与103页的各个delta量对应
    double delta_rot1, delta_trans, delta_rot2;
    double delta_rot1_hat, delta_trans_hat, delta_rot2_hat;
    double delta_rot1_noise, delta_rot2_noise;

    // 相较于于书上，在计算delta _rot1时，多了个判定条件以位移是否小于1cm作为判断条件
    // 如果里程计量得机器人位移小于1cm，delta _rot1就置为0
    if(sqrt(ndata->delta.v[1]*ndata->delta.v[1] + 
            ndata->delta.v[0]*ndata->delta.v[0]) < 0.01)
      delta_rot1 = 0.0;
    else
      delta_rot1 = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]),
                              old_pose.v[2]);
    delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] +
                       ndata->delta.v[1]*ndata->delta.v[1] );
    delta_rot2 = angle_diff(ndata->delta.v[2], delta_rot1);

    // We want to treat backward and forward motion symmetrically for the
    // noise model to be applied below.  The standard model seems to assume forward motion.
    // 取了两次旋转角的锐角
    delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)),
                                fabs(angle_diff(delta_rot1,M_PI)));
    delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)),
                                fabs(angle_diff(delta_rot2,M_PI)));

	// 用一定数量的粒子来表征这个区域，然后对这些粒子进行采样来尽可能多的逼近机器人的真实值
	// 下面还是书上的公式，我们需要的是sample
    for (int i = 0; i < set->sample_count; i++)
    {
        // 指针形式, 会给set的samples都赋值
      pf_sample_t* sample = set->samples + i;

      // Sample pose differences，pf_ran_gaussian函数：从零平均高斯分布中随机抽取，带有标准差sigma
      // 将这两个角度带入pf_ran_gaussian()进行高斯采样，也就是书103页的sample()函数。
      // 对每个粒子，不同的信息就是高斯采样产生的，其他部分不变
      delta_rot1_hat = angle_diff(delta_rot1,
                      pf_ran_gaussian(this->alpha1*delta_rot1_noise*delta_rot1_noise + this->alpha2*delta_trans*delta_trans)  );

      delta_trans_hat = delta_trans - 
              pf_ran_gaussian(this->alpha3*delta_trans*delta_trans +
                              this->alpha4*delta_rot1_noise*delta_rot1_noise +
                              this->alpha4*delta_rot2_noise*delta_rot2_noise);

      delta_rot2_hat = angle_diff(delta_rot2,
                          pf_ran_gaussian(this->alpha1*delta_rot2_noise*delta_rot2_noise +
                        this->alpha2*delta_trans*delta_trans) );

      // Apply sampled update to particle pose, 对初始预测的修正
      sample->pose.v[0] += delta_trans_hat * 
              cos(sample->pose.v[2] + delta_rot1_hat);
      sample->pose.v[1] += delta_trans_hat * 
              sin(sample->pose.v[2] + delta_rot1_hat);
      sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat;
    }
}