amcl运动模型

里程计运动模型

差分机器人运动被分解为旋转，平移，再旋转。定义笛卡尔坐标系，向右为x轴正方向，向上为y轴正方向。

式2: atan2求的是机器人两次位姿连线与x轴的夹角，后面的角度是机器人第一次位姿的朝向与x轴的夹角，取值范围为-pi到pi，其为负数，相减求得机器人第一次旋转角度。
式3: 求平移量。
式4: 机器人两次朝向（都是与x轴的夹角，第一次为负，第二次为正）相减，得到到机器人的角度变换，减去第一次的角度变换，得到第二次角度变换。

sample 得到指定标准差的正太分布。

式5,6,7: 给变化量加噪声，因为里程计的数据是存在噪声的。

式8,9,10: 上一次的位姿加上x,y,th增量得到当前位姿。其中上一次的朝向加上第一次角度变化，得到与x轴的夹角，利用该角度将平移量通过正弦余弦分解得到xy方向上的平移。并当前朝向等于上一次朝向加角度变化量。

  case ODOM_MODEL_DIFF:
  {
    // Implement sample_motion_odometry (Prob Rob p 136)
    double delta_rot1, delta_trans, delta_rot2;
    double delta_rot1_hat, delta_trans_hat, delta_rot2_hat;
    double delta_rot1_noise, delta_rot2_noise;

    // Avoid computing a bearing from two poses that are extremely near each
    // other (happens on in-place rotation).
    //式2 机器人在原地旋转时，会产生侧移的数据，导致delta_rot1很大，而delta_rot1是产生噪声的变量，
    //所以导致粒子快速发散，位姿可能会丢，对于差分模型是不可能发生侧移的。
    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]);
    //式3
    delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] +
                       ndata->delta.v[1]*ndata->delta.v[1]);
    //式4 角度变化量减去第一次角度变化                   
    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)));

    for (int i = 0; i < set->sample_count; i++)
    {
      pf_sample_t* sample = set->samples + i;

	  //式5,6,7: 添加过程噪声，pf_ran_gaussian函数的参数是标准差，标准差和变化量成正比
	  //标准差越大粒子散得越开。
      // Sample pose differences
      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));

      // 式8,9,10: sample->pose.v[2] + delta_rot1_hat 粒子的朝向加上第一次角度变化量，
      // 得到机器人两次位姿连线与x轴的夹角，通过cos，sin将平移分解到xy轴，加上上一次的位姿
      // 得到当前位姿。当前朝向等于上一次朝向加上两次角度变化。
      // 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;
    }
  }
  break;