粒子濾波器兩種初始化

從地圖資料的進行中，有粒子濾波器的初始化，以及用Pose初始值初始化濾波器，這就不得不開始看濾波器部分了。

上篇文章，講到初始化函數pf_alloc、pf_init。在開始說明這些函數前，先了解下粒子濾波器的一些資料結構。

1、粒子濾波器的各種資料結構

濾波器的定義：定義了粒子集pf_sample_set_t sets[2]，粒子初始化函數的指針pf_init_model_fn_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;      

注意，粒子濾波器維護了兩個粒子集，用current_set這個變量來做切換，初始化時的值為0，表示目前集合;具體的切換操作，跳轉到粒子重采樣函數中檢視，pf_update_resample。

粒子集的定義：定義了kdtree，粒子簇，濾波器的統計學參數。這部分的參數含義，以及為什麼定義粒子簇，也需要看《機率機器人》。

// Information for a set of samples   采樣的粒子集：粒子數+單個粒子+kdtree+均值和方差，集合的大小
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;//粒子數，最大粒子數，達到最大粒子數，是kld裡面自适應的實作麼？
  pf_cluster_t *clusters;   //落在同一bin的集合

  // Filter statistics
  pf_vector_t mean;
  pf_matrix_t cov;
  int converged; //相當于pdf裡面的bin
} pf_sample_set_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 single sample  單個采樣粒子：位姿+權重資訊
typedef struct
{
  // Pose represented by this sample
  pf_vector_t pose;

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

這裡列出來，是友善看代碼。因為名字差不多，參數也比較多，在程式調用的時候，對比着看，效率更高。

注釋後面看完代碼再補充，包括粒子簇的作用，權重的計算方式。

2、建立一個濾波器：參數初始化，包括粒子集、單個粒子、粒子簇。

pf_t *pf_alloc(int min_samples, int max_samples,
               double alpha_slow, double alpha_fast,
               pf_init_model_fn_t random_pose_fn, void *random_pose_data)
{
  int i, j;
  pf_t *pf;
  pf_sample_set_t *set;
  pf_sample_t *sample;
  
  srand48(time(NULL));

  pf = calloc(1, sizeof(pf_t));

  pf->random_pose_fn = random_pose_fn;
  pf->random_pose_data = random_pose_data;

  pf->min_samples = min_samples;
  pf->max_samples = max_samples;

  // Control parameters for the population size calculation.  [err] is
  // the max error between the true distribution and the estimated
  // distribution.  [z] is the upper standard normal quantile for (1 -
  // p), where p is the probability that the error on the estimated
  // distrubition will be less than [err].
  pf->pop_err = 0.01;
  pf->pop_z = 3;
  pf->dist_threshold = 0.5; 
  
  pf->current_set = 0;
  for (j = 0; j < 2; j++)
  {
    set = pf->sets + j;//單個粒子集的指針
      
    set->sample_count = max_samples;
    set->samples = calloc(max_samples, sizeof(pf_sample_t));

    for (i = 0; i < set->sample_count; i++)//給每個粒子初始化
    {
      sample = set->samples + i;//單個粒子的指針
      sample->pose.v[0] = 0.0;
      sample->pose.v[1] = 0.0;
      sample->pose.v[2] = 0.0;
      sample->weight = 1.0 / max_samples;//權重一樣，平均值
    }

    // HACK: is 3 times max_samples enough?  因為是3維kdtree
    set->kdtree = pf_kdtree_alloc(3 * max_samples);

    set->cluster_count = 0;
    set->cluster_max_count = max_samples;
    set->clusters = calloc(set->cluster_max_count, sizeof(pf_cluster_t));//設定一個clusters的最大容量是當所有的粒子都在一個cluster時，所占用的空間

    set->mean = pf_vector_zero();//均值
    set->cov = pf_matrix_zero();//協方差
  }

  pf->w_slow = 0.0;
  pf->w_fast = 0.0;

  pf->alpha_slow = alpha_slow;
  pf->alpha_fast = alpha_fast;//看機率機器人

  //set converged to 0
  pf_init_converged(pf);//這裡converged的用法還不清楚

  return pf;
}      

這裡的入參，random_pose_data傳入地圖資料。pf_init_model_fn_t函數指針，在地圖上空白處定義一個位姿點。

3、有指定的位姿後，初始化濾波器：高斯機率分布函數的處理、kdtree插入節點的處理。這裡先不看，知道做了什麼即可，在後面再詳解。

// Initialize the filter using a guassian  用高斯分布初始化粒子
void pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov)
{
  int i;
  pf_sample_set_t *set;
  pf_sample_t *sample;
  pf_pdf_gaussian_t *pdf;
  
  set = pf->sets + pf->current_set;
  
  // Create the kd tree for adaptive sampling
  pf_kdtree_clear(set->kdtree);

  set->sample_count = pf->max_samples;

  pdf = pf_pdf_gaussian_alloc(mean, cov);
    
  // Compute the new sample poses
  for (i = 0; i < set->sample_count; i++)
  {
    sample = set->samples + i;
    sample->weight = 1.0 / pf->max_samples;
    sample->pose = pf_pdf_gaussian_sample(pdf);

    // Add sample to histogram
    pf_kdtree_insert(set->kdtree, sample->pose, sample->weight);
  }

  pf->w_slow = pf->w_fast = 0.0;

  pf_pdf_gaussian_free(pdf);
    
  // Re-compute cluster statistics
  pf_cluster_stats(pf, set); 

  //set converged to 0
  pf_init_converged(pf);

  return;
}      

這裡分析下第2步的函數和這裡的函數的差別，因為都是濾波器的初始化，有啥不一樣呢？

給定初始化Pose值的資料結構：權重和機率參數(均值和協方差)，給的并不是(x,y,theta)

// Pose hypothesis
typedef struct
{
  // Total weight (weights sum to 1)
  double weight;

  // Mean of pose esimate
  pf_vector_t pf_pose_mean;

  // Covariance of pose estimate
  pf_matrix_t pf_pose_cov;

} amcl_hyp_t;      

是以，還要細看這些參數如何轉換成位姿，然後給pf_init初始化

首先了解下pdf的定義：均值，協方差、協方差的迹.(分解協方差的兩個參數的作用還不知道)

// Gaussian PDF info
typedef struct
{
  // Mean, covariance and inverse covariance
  pf_vector_t x;
  pf_matrix_t cx;
  //pf_matrix_t cxi;
  double cxdet;

  // Decomposed covariance matrix (rotation * diagonal)
  pf_matrix_t cr;
  pf_vector_t cd;

  // A random number generator
  //gsl_rng *rng;

} pf_pdf_gaussian_t;      

3.1看下pf_pdf_gaussian_alloc函數：初始化操作，計算pdf值

pf_pdf_gaussian_t *pf_pdf_gaussian_alloc(pf_vector_t x, pf_matrix_t cx)
{
  pf_matrix_t cd;
  pf_pdf_gaussian_t *pdf;

  pdf = calloc(1, sizeof(pf_pdf_gaussian_t));

  pdf->x = x;
  pdf->cx = cx;
  //pdf->cxi = pf_matrix_inverse(cx, &pdf->cxdet);

  // Decompose the convariance matrix into a rotation
  // matrix and a diagonal matrix.
  pf_matrix_unitary(&pdf->cr, &cd, pdf->cx);
  pdf->cd.v[0] = sqrt(cd.m[0][0]);
  pdf->cd.v[1] = sqrt(cd.m[1][1]);
  pdf->cd.v[2] = sqrt(cd.m[2][2]);

  // Initialize the random number generator
  //pdf->rng = gsl_rng_alloc(gsl_rng_taus);
  //gsl_rng_set(pdf->rng, ++pf_pdf_seed);
  srand48(++pf_pdf_seed);

  return pdf;
}      

3.2看下pf_pdf_gaussian_sample函數：其實看到這裡，就已經知道答案了。因為其他的之都是一樣的，就是這個采樣函數不一樣。這裡采用的高斯分布函數産生的初始位姿粒子，在Pose附近

// Generate a sample from the pdf.
pf_vector_t pf_pdf_gaussian_sample(pf_pdf_gaussian_t *pdf)//高斯分布采樣
{
  int i, j;
  pf_vector_t r;
  pf_vector_t x;

  // Generate a random vector
  for (i = 0; i < 3; i++)
  {
    //r.v[i] = gsl_ran_gaussian(pdf->rng, pdf->cd.v[i]);
    r.v[i] = pf_ran_gaussian(pdf->cd.v[i]);
  }

  for (i = 0; i < 3; i++)
  {
    x.v[i] = pdf->x.v[i];
    for (j = 0; j < 3; j++)
      x.v[i] += pdf->cr.m[i][j] * r.v[j];
  } 
  
  return x;
}      

3.3看下pf_cluster_stats函數：重新計算粒子集的機率統計參數，因為在第2步中，均值和協方差都是0

// Re-compute the cluster statistics for a sample set
void pf_cluster_stats(pf_t *pf, pf_sample_set_t *set)
{
  int i, j, k, cidx;
  pf_sample_t *sample;
  pf_cluster_t *cluster;
  
  // Workspace
  double m[4], c[2][2];
  size_t count;
  double weight;

  // Cluster the samples  聚類采樣的粒子
  pf_kdtree_cluster(set->kdtree);
  
  // Initialize cluster stats  初始化統計資料
  set->cluster_count = 0;

  for (i = 0; i < set->cluster_max_count; i++)
  {
    cluster = set->clusters + i;
    cluster->count = 0;
    cluster->weight = 0;
    cluster->mean = pf_vector_zero();
    cluster->cov = pf_matrix_zero();

    for (j = 0; j < 4; j++)
      cluster->m[j] = 0.0;
    for (j = 0; j < 2; j++)
      for (k = 0; k < 2; k++)
        cluster->c[j][k] = 0.0;
  }

  // Initialize overall filter stats
  count = 0;
  weight = 0.0;
  set->mean = pf_vector_zero();
  set->cov = pf_matrix_zero();
  for (j = 0; j < 4; j++)
    m[j] = 0.0;
  for (j = 0; j < 2; j++)
    for (k = 0; k < 2; k++)
      c[j][k] = 0.0;
  
  // Compute cluster stats
  for (i = 0; i < set->sample_count; i++)
  {
    sample = set->samples + i;

    //printf("%d %f %f %f\n", i, sample->pose.v[0], sample->pose.v[1], sample->pose.v[2]);

    // Get the cluster label for this sample  通過位姿找到聚類的辨別
    cidx = pf_kdtree_get_cluster(set->kdtree, sample->pose);
    assert(cidx >= 0);
    if (cidx >= set->cluster_max_count)
      continue;
    if (cidx + 1 > set->cluster_count)
      set->cluster_count = cidx + 1;
    
    cluster = set->clusters + cidx;

    cluster->count += 1;
    cluster->weight += sample->weight;

    count += 1;
    weight += sample->weight;

    // Compute mean
    cluster->m[0] += sample->weight * sample->pose.v[0];
    cluster->m[1] += sample->weight * sample->pose.v[1];
    cluster->m[2] += sample->weight * cos(sample->pose.v[2]);
    cluster->m[3] += sample->weight * sin(sample->pose.v[2]);

    m[0] += sample->weight * sample->pose.v[0];
    m[1] += sample->weight * sample->pose.v[1];
    m[2] += sample->weight * cos(sample->pose.v[2]);
    m[3] += sample->weight * sin(sample->pose.v[2]);

    // Compute covariance in linear components
    for (j = 0; j < 2; j++)
      for (k = 0; k < 2; k++)
      {
        cluster->c[j][k] += sample->weight * sample->pose.v[j] * sample->pose.v[k];
        c[j][k] += sample->weight * sample->pose.v[j] * sample->pose.v[k];
      }
  }

  // Normalize
  for (i = 0; i < set->cluster_count; i++)
  {
    cluster = set->clusters + i;
        
    cluster->mean.v[0] = cluster->m[0] / cluster->weight;
    cluster->mean.v[1] = cluster->m[1] / cluster->weight;
    cluster->mean.v[2] = atan2(cluster->m[3], cluster->m[2]);

    cluster->cov = pf_matrix_zero();

    // Covariance in linear components
    for (j = 0; j < 2; j++)
      for (k = 0; k < 2; k++)
        cluster->cov.m[j][k] = cluster->c[j][k] / cluster->weight -
          cluster->mean.v[j] * cluster->mean.v[k];

    // Covariance in angular components; I think this is the correct
    // formula for circular statistics.
    cluster->cov.m[2][2] = -2 * log(sqrt(cluster->m[2] * cluster->m[2] +
                                         cluster->m[3] * cluster->m[3]));

    //printf("cluster %d %d %f (%f %f %f)\n", i, cluster->count, cluster->weight,
           //cluster->mean.v[0], cluster->mean.v[1], cluster->mean.v[2]);
    //pf_matrix_fprintf(cluster->cov, stdout, "%e");
  }

  // Compute overall filter stats
  set->mean.v[0] = m[0] / weight;
  set->mean.v[1] = m[1] / weight;
  set->mean.v[2] = atan2(m[3], m[2]);

  // Covariance in linear components
  for (j = 0; j < 2; j++)
    for (k = 0; k < 2; k++)
      set->cov.m[j][k] = c[j][k] / weight - set->mean.v[j] * set->mean.v[k];

  // Covariance in angular components; I think this is the correct
  // formula for circular statistics.
  set->cov.m[2][2] = -2 * log(sqrt(m[2] * m[2] + m[3] * m[3]));

  return;
}      

初始化後，後面做了什麼事呢？看到pf的庫檔案中，還有6個函數需要繼續解讀。

// Initialize the filter using some model
void pf_init_model(pf_t *pf, pf_init_model_fn_t init_fn, void *init_data);

// Update the filter with some new action
void pf_update_action(pf_t *pf, pf_action_model_fn_t action_fn, void *action_data);

// Update the filter with some new sensor observation
void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data);

// Resample the distribution
void pf_update_resample(pf_t *pf);

// Compute the statistics for a particular cluster.  Returns 0 if
// there is no such cluster.
int pf_get_cluster_stats(pf_t *pf, int cluster, double *weight,
                         pf_vector_t *mean, pf_matrix_t *cov);

//calculate if the particle filter has converged - 
//and sets the converged flag in the current set and the pf 
int pf_update_converged(pf_t *pf);      

這篇文章就不講了，因為是接着上一篇地圖資料處理的文章的疑問來的。地圖資料部分處理完，開始看主流程函數laserReceived。然後再看pf的這剩下的幾個函數是怎麼處理的噢～

小結一下：

pf_alloc和pf_init的差別是：

1、前者用drand48函數，在地圖的空白區域産生随機均勻分布的粒子，後者是用pf_pdf_gaussian_sample函數，在給定位姿Pose的附近産生的随機正态分布的粒子;

2、前者的cluster的均值和協方差是0，後者pf_cluster_stats函數根據pdf的值更新了cluster的均值和協方差;

3、前者是程式必須要執行的操作，後者隻在特定場景用，比如給定了初始值，才調用。給定初值便于濾波器快速收斂。