amcl運用在地圖地位中的演算法（見Probabilistic Robotics chapter8 與 基 於多假設跟蹤 的移動機器人 自適應蒙特卡羅定位研 究，mcl具體推導過程可參考http://blog.csdn.net/heyijia0327/article/details/40899819）

在熟悉原始碼的時候需要弄清楚幾個結構體所包含的資訊

typedef struct
{
  // Pose represented by this sample
  pf_vector_t pose;

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


// Information for a cluster of samples
typedef struct
{
  // Number of samples
  int count;

  // Total weight of samples in this cluster
  double weight;

  // Cluster statistics
  pf_vector_t mean;
  pf_matrix_t cov;

  // Workspace
  double m[4], c[2][2];
  
} pf_cluster_t;


// Information for a set of samples
typedef struct _pf_sample_set_t
{
  // The samples
  int sample_count;
  pf_sample_t *samples;

  // A kdtree encoding the histogram
  pf_kdtree_t *kdtree;

  // Clusters
  int cluster_count, cluster_max_count;
  pf_cluster_t *clusters;

  // Filter statistics
  pf_vector_t mean;
  pf_matrix_t cov;
  int converged; 
} pf_sample_set_t;


// Information for an entire filter
typedef struct _pf_t
{
  // This min and max number of samples
  int min_samples, max_samples;

  // Population size parameters
  double pop_err, pop_z;
  
  // The sample sets.  We keep two sets and use [current_set]
  // to identify the active set.
  int current_set;
  pf_sample_set_t sets[2];

  // Running averages, slow and fast, of likelihood
  double w_slow, w_fast;

  // Decay rates for running averages
  double alpha_slow, alpha_fast;

  // Function used to draw random pose samples
  pf_init_model_fn_t random_pose_fn;
  void *random_pose_data;

  double dist_threshold; //distance threshold in each axis over which the pf is considered to not be converged
  int converged; 
} pf_t;
 

在原始碼中，對定位整體上的處理在amcl_node.cpp中，主要流程為：

   1： 首先獲取地圖  呼叫 handleMapMessa（） 進行地圖轉換 ，記錄obsta座標 ，初始化pf_t 結構體等操作

   2： 獲取初始位置 呼叫 handinitialpose()  進而呼叫 pf_init()處理初始位置座標

   3： 更新機器人位置座標 bindLaserRecieved（）

          a： 獲取laser對應於baselink的座標

          b：獲取baselink對應於odom的座標

          c：根據里程計的變化值+高斯噪音 更新 pf_t中samples的內里程計值

                odom->updateAction()

          d：根據當前雷達資料更新各里程計對應的權值weights  （觀測模型   p（z|x）=   1/sqrt(2*pi*σ^2)) * exp(-(z-μ  )^2/(2*σ^2))）

               laser_[laser_index]->updateSensor（）

                具體演算法見（Probabilistic Robotics chapter6）

               如：LikelihoodFieldModel 模型，演算法為：

             e：獲取權值最高的座標點  進行聚類，對高權值得cluster內的點 求均值即為當前機器人所在位置座標

                    pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean, &pose_cov)

             f：更新取樣里程計值

                   pf_update_resample(pf_)

          【具體見（Tutorial on Monte Carlo Techniques chapter7）】

       amcl定位與mcl定位的主要區別在於 αslow，αfast 不為0， 會地圖上隨機取樣點（里程計） ，但當地圖較大且相似地方區域的情況下，可能會造成機器人座標跳變