粒子滤波器两种初始化

从地图数据的处理中，有粒子滤波器的初始化，以及用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 计算这个cluster标识相同的所有样本的统计参数
  for (i = 0; i < set->sample_count; i++)
  {
    sample = set->samples + i;

    //printf("%d %f %f %f
", 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

    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)
", 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;
}

首先要弄清楚cluster的概念。把值相差一定大小的粒子分割成多个簇，分别计算每个簇的均值和写方差，权重大小，就可以知道，预测的机器人位姿在哪个区间。而AMCL的粒子簇的产生，是根据kdtree以及设定的分割粒度来确定的。

所以，这个函数里有怎么去设置不同的簇和计算不同簇的统计学参数。分别是pf_kdtree_cluster、pf_kdtree_get_cluster函数。这两个函数，后面分析kdtree时会详解。这里就不展开了。

初始化后，后面做了什么事呢？看到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、前者是程序必须要执行的操作，后者只在特定场景用，比如给定了初始值，才调用。给定初值便于滤波器快速收敛。