从地图数据的处理中，有粒子滤波器的初始化，以及用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、前者是程序必须要执行的操作，后者只在特定场景用，比如给定了初始值，才调用。给定初值便于滤波器快速收敛。