0. 寫在最前面

這篇文章記錄下自己在閱讀amcl原始碼過程中的一些理解，如有不妥，歡迎評論或私信。

本文中所有程式碼因為篇幅等問題，都只給出主要部分，詳細的自己下載下來對照著看。

作者是在校研究生，會長期跟新自己學習ROS以及SLAM過程中的一些理解，喜歡的話歡迎點個關注。

1. amcl是幹什麼的

  Amcl is a probabilistic localization system for a robot moving in 2D. It implements the adaptive (or KLD-sampling) Monte Carlo localization approach (as described by Dieter Fox), which uses a particle filter to track the pose of a robot against a known map.

以上是官網的介紹，說白了就是2D的概率定位系統，輸入鐳射雷達資料、里程計資料，輸出機器人在地圖中的位姿。用的是自適應蒙特卡洛定位方法，這個方法是在已知地圖中使用粒子濾波方法得到位姿的。

如下圖所示，如果里程計沒有誤差，完美的情況下，我們可以直接使用里程計資訊（上半圖）推算出機器人（base_frame）相對里程計座標系的位置。但現實情況，里程計存在漂移以及無法忽略的累計誤差，所以AMCL採用下半圖的方法，即先根據里程計資訊初步定位base_frame，然後通過測量模型得到base_frame相對於map_frame（全域性地圖座標系），也就知道了機器人在地圖中的位姿。（注意，這裡雖然估計的是base到map的轉換，但最後釋出的是map到odom的轉換，可以理解為里程計的漂移。）

2. 總體情況

2.1 CMakeLists

    研究一個ROS包，肯定要先看CMakeLists。我們可以看到，這個包會生成

三個庫：

• amcl_pf
• amcl_map
• amcl_sensors

一個節點：

• amcl

2.2 其中amcl的訂閱與釋出：

釋出話題：

1. amcl_pose： geometry_msgs::PoseWithCovarianceStamped，後驗位姿+一個6*6的協方差矩陣（xyz+三個轉角）
2. particlecloud：geometry_msgs::PoseArray，粒子位姿的陣列
3. 一個15秒的定時器：AmclNode::checkLaserReceived，檢查 15上一次收到鐳射雷達資料至今是否超過15秒，如超過則報錯。

釋出服務：

1. global_localization：&AmclNode::globalLocalizationCallback，這裡是沒有給定初始位姿的情況下在全域性範圍內初始化粒子位姿，該Callback呼叫pf_init_model，然後呼叫AmclNode::uniformPoseGenerator在地圖的free點隨機生成pf->max_samples個粒子
2. request_nomotion_update：&AmclNode::nomotionUpdateCallback沒運動模型更新的情況下也暫時更新粒子群
3. set_map：&AmclNode::setMapCallback
4. dynamic_reconfigure::Server動態引數配置器。

訂閱話題：

1. scan_topic_：sensor_msgs::LaserScan，AmclNode::laserReceived，這裡是tf訂閱，轉換到odom_frame_id_
2. initialpose：AmclNode::initialPoseReceived，這個應該就是訂閱rviz中給的初始化位姿，呼叫AmclNode::handleInitialPoseMessage，只接受global_frame_id_（一般為map）的座標，並重新生成粒子
3. map：AmclNode::mapReceived這個在use_map_topic_的時候才訂閱，否則requestMap();我這裡也沒有訂閱，因為只使用了一個固定的地圖。

3.amcl_node.cpp

    這個檔案實現了上述的amcl節點功能。

3.1 main

int main(int argc, char** argv)
{
  ros::init(argc, argv, "amcl");
  ros::NodeHandle nh;
  // Override default sigint handler
  signal(SIGINT, sigintHandler);
  // Make our node available to sigintHandler
  amcl_node_ptr.reset(new AmclNode());
  if (argc == 1)
  {
    // run using ROS input
    ros::spin();
  }
 }

沒啥好說的，主要就是定義了amcl節點，初始化了一個AmclNode的類物件，最關鍵的中斷函式配置都在該類的建構函式中實現。

3.2 AmclNode

AmclNode::AmclNode()
{
  boost::recursive_mutex::scoped_lock l(configuration_mutex_);
//測量模型選擇，具體參考《概率機器人》第6章內容，這裡預設的likelihood_field是6.4節的似然域模型
  std::string tmp_model_type;
  private_nh_.param("laser_model_type", tmp_model_type, std::string("likelihood_field"));
  
//機器人模型選擇，具體參考《概率機器人》5.4節里程計運動模型內容，這裡預設的diff應該是差分型（兩輪驅動）的機器人？，另外一個是全向機器人？
  private_nh_.param("odom_model_type", tmp_model_type, std::string("diff"));
//從引數伺服器中獲取初始位姿及初始分佈
  updatePoseFromServer();
//定義話題及訂閱等，具體在本文第2章有講
  pose_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose", 2, true);
}

3.3 requestMap

這裡請求服務static_server提供map，然後呼叫handleMapMessage處理地圖資訊。這裡的地圖型別是nav_msgs::OccupancyGrid，這裡不介紹，有興趣自己去看資料型別。然後獲取初始位姿、初始化粒子、里程計資訊等。

AmclNode::requestMap()
{
//一直請求服務static_map直到成功，該服務在map_server這個包的map_server節點中進行定義
  while(!ros::service::call("static_map", req, resp))
  {
    ROS_WARN("Request for map failed; trying again...");
    ros::Duration d(0.5);
    d.sleep();
  }
  handleMapMessage( resp.map );
}

AmclNode::handleMapMessage(const nav_msgs::OccupancyGrid& msg)
{
//free相應的指標
  freeMapDependentMemory();
//轉換成標準地圖，0->-1(不是障礙);100->+1（障礙）;else->0（不明）
  map_ = convertMap(msg);

//將不是障礙的點的座標儲存下來
#if NEW_UNIFORM_SAMPLING
  // Index of free space
  free_space_indices.resize(0);
  for(int i = 0; i < map_->size_x; i++)
    for(int j = 0; j < map_->size_y; j++)
      if(map_->cells[MAP_INDEX(map_,i,j)].occ_state == -1)
        free_space_indices.push_back(std::make_pair(i,j));
#endif
  // Create the particle filter，定義了一個回撥，尚未清除幹啥
  pf_ = pf_alloc(min_particles_, max_particles_,
                 alpha_slow_, alpha_fast_,
                 (pf_init_model_fn_t)AmclNode::uniformPoseGenerator,
                 (void *)map_);
  // 從引數伺服器獲取初始位姿及方差放到pf中
  updatePoseFromServer();

  // 定義里程計與鐳射雷達並初始化資料
  // Odometry
  delete odom_;
  odom_ = new AMCLOdom();
  ROS_ASSERT(odom_);
  odom_->SetModel( odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_ );
  // Laser
  delete laser_;
  laser_ = new AMCLLaser(max_beams_, map_);

  // In case the initial pose message arrived before the first map,
  // try to apply the initial pose now that the map has arrived.
  applyInitialPose();
}

3.4 laserReceived

    其中變數pose是base相對於odom的位姿；pf_odom_pose_則是上一時刻base相對於odom的位姿，用於後續得到機器人的相對運動程度。

AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan)
{ 
  // Do we have the base->base_laser Tx yet?感覺這裡是支援多個鐳射雷達的，找到當前響應的鐳射雷達之前儲存的資訊，
//如相對於base的轉換，是否更新等，使用map結構直接通過id來找到對應序號即可
//如果之前沒有備案則在map結構中備案，然後存到frame_to_laser_及lasers_中下次備用
  if(frame_to_laser_.find(laser_scan->header.frame_id) == frame_to_laser_.end())
  {
    frame_to_laser_[laser_scan->header.frame_id] = laser_index;
  } else {
    // we have the laser pose, retrieve laser index
    laser_index = frame_to_laser_[laser_scan->header.frame_id];
  }

  // Where was the robot when this scan was taken?獲得base在鐳射雷達掃描時候相對於odom的相對位姿
  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_)
  {
    // Compute change in pose
    //delta = pf_vector_coord_sub(pose, 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]);

    // See if we should update the filter
    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;
  if(!pf_init_)
  {
    // Pose at last filter update
    pf_odom_pose_ = pose;
    // Filter is now initialized
    pf_init_ = true;
  }
  // If the robot has moved, update the filter
  else if(pf_init_ && lasers_update_[laser_index])//如果已經初始化並需要更新則更新運動模型
  {
//這是amcl_odom.cpp中最重要的一個函式，實現了用運動模型來更新現有的每一個粒子的位姿（這裡得到的只是當前時刻的先驗位姿）
    // Use the action data to update the filter
    odom_->UpdateAction(pf_, (AMCLSensorData*)&odata);
  }

  bool resampled = false;
  // If the robot has moved, update the filter
  if(lasers_update_[laser_index])
  {//接下來一大片都是對原始鐳射雷達資料進行處理，轉換到AMCLLaserData。包括角度最小值、增量到base_frame的轉換、測距距離最大值、最小值。
    //具體看程式碼，篇幅限制不詳細列了
    try
    {
      tf_->transformQuaternion(base_frame_id_, min_q, min_q);
      tf_->transformQuaternion(base_frame_id_, inc_q, inc_q);
    }
  
    ldata.range_max = std::min(laser_scan->range_max, (float)laser_max_range_);
    range_min = std::max(laser_scan->range_min, (float)laser_min_range_);
        
    ldata.ranges = new double[ldata.range_count][2];
    for(int i=0;i<ldata.range_count;i++)
    {
      // amcl doesn't (yet) have a concept of min range.  So we'll map short
      // readings to max range.//鐳射雷達傳上來的資料只標記了最大值最小值，但是沒做處理，直接將原始資料傳上來，
      if(laser_scan->ranges[i] <= range_min)//這裡將最小值當最大值處理，因為在類似likelihood_field模型中，會直接將最大值丟棄
        ldata.ranges[i][0] = ldata.range_max;
    }
//注意這裡是amcl_laser.cpp的UpdateSensor，不是amcl_sensor.cpp的。通過判斷前面設定的測量模型呼叫pf_update_sensor，
//該函式需要傳入相應模型的處理函式，這裡所有的測量模型在《概率機器人》的第六章有詳細講，具體自己看，後面只針對自己使用的likelihood_field模型講一下
//pf_update_sensor實現對所有粒子更新權重，並歸一化、計算出《概率機器人》8.3.5的失效恢復的長期似然和短期似然
    lasers_[laser_index]->UpdateSensor(pf_, (AMCLSensorData*)&ldata);

    lasers_update_[laser_index] = false;
    pf_odom_pose_ = pose;
//多少次鐳射雷達回撥之後進行重取樣唄，我這裡resample_interval_=0.5，只有一個鐳射雷達，每次都更新。
    // Resample the particles
    if(!(++resample_count_ % resample_interval_))
    {
//按照一定的規則重取樣粒子，包括前面說的失效恢復、粒子權重等，然後放入到kdtree，暫時先理解成關於位姿的二叉樹，
//然後進行聚類，得到均值和方差等資訊,個人理解就是將相近的一堆粒子融合成一個粒子了，沒必要維持太多相近的@TODO
      pf_update_resample(pf_);
      resampled = true;
    }

    // Publish the resulting cloud
    // TODO: set maximum rate for publishing
    if (!m_force_update) {
     //將新粒子釋出到全域性座標系下，一般是map
      particlecloud_pub_.publish(cloud_msg);
    }
  }


  if(resampled || force_publication)
  {
    //遍歷所有粒子簇，找出權重均值最大的簇，其平均位姿就是我們要求的機器人後驗位姿，到此一次迴圈已經所有完成
    for(int hyp_count = 0;
        hyp_count < pf_->sets[pf_->current_set].cluster_count; hyp_count++)
    {
      if (!pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean, &pose_cov))
      {
        break;
      }
      hyps[hyp_count].weight = weight;
      hyps[hyp_count].pf_pose_mean = pose_mean;
      hyps[hyp_count].pf_pose_cov = pose_cov;

      if(hyps[hyp_count].weight > max_weight)
      {
        max_weight = hyps[hyp_count].weight;
        max_weight_hyp = hyp_count;
      }
    }

    //將位姿、粒子集、協方差矩陣等進行更新、釋出
    if(max_weight > 0.0)
    {
     geometry_msgs::PoseWithCovarianceStamped p;
      // Fill in the header
      p.header.frame_id = global_frame_id_;
      p.header.stamp = laser_scan->header.stamp;
      // Copy in the pose
      p.pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0];
      p.pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1];
      tf::quaternionTFToMsg(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
                            p.pose.pose.orientation);
      // Copy in the covariance, converting from 3-D to 6-D
      pf_sample_set_t* set = pf_->sets + pf_->current_set;
      for(int i=0; i<2; i++)
      {
        for(int j=0; j<2; j++)
        {
          p.pose.covariance[6*i+j] = set->cov.m[i][j];
        }
      }
      p.pose.covariance[6*5+5] = set->cov.m[2][2];
    
      pose_pub_.publish(p);
      last_published_pose = p;
//這裡就是所說的，map～base減去odom～base得到map～odom，最後釋出的是map～odom。
  // subtracting base to odom from map to base and send map to odom instead
      tf::Stamped<tf::Pose> odom_to_map;
      try
      {
        tf::Transform tmp_tf(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
                             tf::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],
                                         hyps[max_weight_hyp].pf_pose_mean.v[1],
                                         0.0));
        tf::Stamped<tf::Pose> tmp_tf_stamped (tmp_tf.inverse(),
                                              laser_scan->header.stamp,
                                              base_frame_id_);
        this->tf_->transformPose(odom_frame_id_,
                                 tmp_tf_stamped,
                                 odom_to_map);
      }
      catch(tf::TransformException)
      {
        ROS_DEBUG("Failed to subtract base to odom transform");
        return;
      }


      latest_tf_ = tf::Transform(tf::Quaternion(odom_to_map.getRotation()),
                                 tf::Point(odom_to_map.getOrigin()));
      latest_tf_valid_ = true;


      if (tf_broadcast_ == true)
      {
        // We want to send a transform that is good up until a
        // tolerance time so that odom can be used
        ros::Time transform_expiration = (laser_scan->header.stamp +
                                          transform_tolerance_);
        tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
                                            transform_expiration,
                                            global_frame_id_, odom_frame_id_);
        this->tfb_->sendTransform(tmp_tf_stamped);
        sent_first_transform_ = true;
      }
    }
    else
    {
      ROS_ERROR("No pose!");
    }
  }
  else if(latest_tf_valid_)//if(resampled || force_publication)
  {
   
  }


}

3.5 AMCLOdom::UpdateAction

    首先獲取上一幀的位姿，然後根據自己前面選擇的機器人模型進行運動模型更新。這裡由於我自己的模型是ODOM_MODEL_DIFF，所以只針對這個模型進行講解，剩下的原理應該一樣，細節上有區別，自己看。程式碼中有講到，利用到的演算法是《Probabilistic Robotics 2》136頁的內容，如果是Probabilistic Robotics 1或者中文版其實就是5.4節中的取樣演算法，這裡不再細述。

    我直白一點的理解就是，已知上一時刻所有粒子的後驗位姿、上一時刻到現在兩個輪子的編碼器值，給所有粒子隨機取樣當前時刻的先驗位姿。為什麼是隨機，不是確定的直接從編碼器計算得到呢？因為編碼器等存在誤差，所以取樣到的每個粒子的先驗位姿都是不確定的，但理論上是服從一個分佈的（這裡是正態，書本上給出三種分佈），跟實際運動有關，如果運動越大、則取樣到的跟直接里程計計算得到的值就越有可能相差的比較大（演算法上表示為方差越大）。

// Apply the action model
bool AMCLOdom::UpdateAction(pf_t *pf, AMCLSensorData *data)
{
  set = pf->sets + pf->current_set;
  pf_vector_t old_pose = pf_vector_sub(ndata->pose, ndata->delta);

  switch( this->model_type )
  {
   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;
//計算里程計得到的ut，即用旋轉、直線運動和另一個旋轉來表示相對運動
    // Avoid computing a bearing from two poses that are extremely near each
    // other (happens on in-place rotation).
    if(sqrt(ndata->delta.v[1]*ndata->delta.v[1] + //如果原地旋轉的情況，定義第一個旋轉為0，認為所有旋轉都是第二個旋轉做的
            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);
//這裡作者比書本多考慮了一種情況，就是機器人旋轉小角度，然後往後走的情況，比如旋轉-20°，然後往後走，計算出來可能是160°，
//但實際只轉了20°。按書本可能是比較大的方差（160°），不太準確，但這裡的話還是按照比較小的方差來取樣。
//但我表示懷疑，萬一真的轉了160°再前向運動的話怎麼辦？還是用比較小的方差來取樣，可能會使得采樣的準確率下降。
//實際中，編碼器取樣率很高，且設定了一個很小的變化角度就進行運動模型更新，所以我說的這種情況幾乎不會發生
    // 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;

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

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

3.6 LikelihoodFieldModel

    演算法原理請參看《概率機器人》6.4節，這裡會給每個粒子（代表一個位姿）計算概率、更新概率，然後返回所有粒子概率的總和。

double AMCLLaser::LikelihoodFieldModel(AMCLLaserData *data, pf_sample_set_t* set)
{
  // Compute the sample weights遍歷所有鐳射雷達資料點，
  for (j = 0; j < set->sample_count; j++)
  {
    sample = set->samples + j;
    pose = sample->pose;//遍歷每個粒子，這是粒子對應的位姿，是經運動模型更新後先驗位姿

    // Take account of the laser pose relative to the robot
//這個應該是通過機器人與全域性座標系的位姿（每個粒子的位姿），計算鐳射雷達相對於全域性座標系的位姿。方便後續將鐳射雷達掃描的終點轉換到全域性座標系
//點進去pf_vector_coord_add這個函式，b對應的就是《概率機器人》P128,6.4第一個公式中的機器人在t時刻的位姿，a代表“與機器人固連的感測器區域性座標系位置”
    pose = pf_vector_coord_add(self->laser_pose, pose);//這裡的laser_pose其實是laserReceived函式一開始初始化每個鐳射雷達時得到的鐳射雷達在base中的位姿

    p = 1.0;

    // Pre-compute a couple of things
    double z_hit_denom = 2 * self->sigma_hit * self->sigma_hit;//測量噪聲的方差
    double z_rand_mult = 1.0/data->range_max;//無法解釋的隨機測量的分母
//因為時間問題，並不是全部點都進行似然計算的，這裡只是間隔地選點，我這裡設定的是一共選擇60個點～～～
    step = (data->range_count - 1) / (self->max_beams - 1);
    for (i = 0; i < data->range_count; i += step)
    {
      obs_range = data->ranges[i][0];
      obs_bearing = data->ranges[i][1];

//繼續上面《概率機器人》P128的後半段公式，得到鐳射雷達點最遠端的世界座標
      // 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]);

//這是提前計算好的最近距離，計算函式在map_cspace.cpp的map_update_cspace中實現遍歷計算，該函式是被AMCLLaser::SetModelLikelihoodField呼叫
//而這個函式則只在AmclNode::handleMapMessage中呼叫？？？而這個handle在多個地方呼叫，暫時未清楚@TODO      
      // 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;
//書本演算法流程的公式
      // Gaussian model
      // NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma)
      pz += self->z_hit * exp(-(z * z) / z_hit_denom);
      // Part 2: random measurements
      pz += self->z_rand * z_rand_mult;


      // TODO: outlier rejection for short readings


      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;//這裡有點不太懂。。。理論上應該是上面註釋掉的公式。這裡應該是為了防止噪聲，比如某個出現錯誤的0，其餘都不管用了
    }//每個粒子的所有鐳射雷達點迴圈


    sample->weight *= p;
    total_weight += sample->weight;
  }//粒子迴圈


  return(total_weight);
}

4. 參考