PF部分代码解读

// 单个粒子数据结构
typedef struct
{
  // 粒子状态
  pf_vector_t pose;

  // 粒子权重
  double weight;

} pf_sample_t;

// Information for a cluster of samples
// 粒子聚类
typedef struct
{
  // 粒子数量
  int count;

  // 该聚类中的粒子总权重
  double weight;

  // 聚类统计量
  pf_vector_t mean;
  pf_matrix_t cov;

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

} pf_cluster_t;

// 一组样本粒子数据结构
typedef struct _pf_sample_set_t
{
  // The samples
  int sample_count;
  pf_sample_t *samples;

  // A kdtree encoding the histogram
  pf_kdtree_t *kdtree;

  // 聚类数据
  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;

// 整个滤波器数据结构
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;