Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * pf.h: Simple particle filter for localization 00004 * 00005 * Created: Wed May 16 16:04:41 2012 00006 * Copyright 2000 Brian Gerkey 00007 * 2000 Kasper Stoy 00008 * 2012 Tim Niemueller [www.niemueller.de] 00009 ****************************************************************************/ 00010 00011 /* This program is free software; you can redistribute it and/or modify 00012 * it under the terms of the GNU General Public License as published by 00013 * the Free Software Foundation; either version 2 of the License, or 00014 * (at your option) any later version. 00015 * 00016 * This program is distributed in the hope that it will be useful, 00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00019 * GNU Library General Public License for more details. 00020 * 00021 * Read the full text in the LICENSE.GPL file in the doc directory. 00022 */ 00023 00024 /* From: 00025 * Player - One Hell of a Robot Server (LGPL) 00026 * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 00027 * gerkey@usc.edu kaspers@robotics.usc.edu 00028 */ 00029 /************************************************************************** 00030 * Desc: Simple particle filter for localization. 00031 * Author: Andrew Howard 00032 * Date: 10 Dec 2002 00033 *************************************************************************/ 00034 00035 #ifndef PF_H 00036 #define PF_H 00037 00038 #include "pf_vector.h" 00039 #include "pf_kdtree.h" 00040 00041 #ifdef __cplusplus 00042 extern "C" { 00043 #endif 00044 00045 /// @cond EXTERNAL 00046 00047 // Forward declarations 00048 struct _pf_t; 00049 struct _rtk_fig_t; 00050 struct _pf_sample_set_t; 00051 00052 // Function prototype for the initialization model; generates a sample pose from 00053 // an appropriate distribution. 00054 typedef pf_vector_t (*pf_init_model_fn_t) (void *init_data); 00055 00056 // Function prototype for the action model; generates a sample pose from 00057 // an appropriate distribution 00058 typedef void (*pf_action_model_fn_t) (void *action_data, 00059 struct _pf_sample_set_t* set); 00060 00061 // Function prototype for the sensor model; determines the probability 00062 // for the given set of sample poses. 00063 typedef double (*pf_sensor_model_fn_t) (void *sensor_data, 00064 struct _pf_sample_set_t* set); 00065 00066 00067 // Information for a single sample 00068 typedef struct 00069 { 00070 // Pose represented by this sample 00071 pf_vector_t pose; 00072 00073 // Weight for this pose 00074 double weight; 00075 00076 } pf_sample_t; 00077 00078 00079 // Information for a cluster of samples 00080 typedef struct 00081 { 00082 // Number of samples 00083 int count; 00084 00085 // Total weight of samples in this cluster 00086 double weight; 00087 00088 // Cluster statistics 00089 pf_vector_t mean; 00090 pf_matrix_t cov; 00091 00092 // Workspace 00093 double m[4], c[2][2]; 00094 00095 } pf_cluster_t; 00096 00097 00098 // Information for a set of samples 00099 typedef struct _pf_sample_set_t 00100 { 00101 // The samples 00102 int sample_count; 00103 pf_sample_t *samples; 00104 00105 // A kdtree encoding the histogram 00106 pf_kdtree_t *kdtree; 00107 00108 // Clusters 00109 int cluster_count, cluster_max_count; 00110 pf_cluster_t *clusters; 00111 00112 // Filter statistics 00113 pf_vector_t mean; 00114 pf_matrix_t cov; 00115 00116 } pf_sample_set_t; 00117 00118 00119 // Information for an entire filter 00120 typedef struct _pf_t 00121 { 00122 // This min and max number of samples 00123 int min_samples, max_samples; 00124 00125 // Population size parameters 00126 double pop_err, pop_z; 00127 00128 // The sample sets. We keep two sets and use [current_set] 00129 // to identify the active set. 00130 int current_set; 00131 pf_sample_set_t sets[2]; 00132 00133 // Running averages, slow and fast, of likelihood 00134 double w_slow, w_fast; 00135 00136 // Decay rates for running averages 00137 double alpha_slow, alpha_fast; 00138 00139 // Function used to draw random pose samples 00140 pf_init_model_fn_t random_pose_fn; 00141 void *random_pose_data; 00142 } pf_t; 00143 00144 00145 // Create a new filter 00146 pf_t *pf_alloc(int min_samples, int max_samples, 00147 double alpha_slow, double alpha_fast, 00148 pf_init_model_fn_t random_pose_fn, void *random_pose_data); 00149 00150 // Free an existing filter 00151 void pf_free(pf_t *pf); 00152 00153 // Initialize the filter using a guassian 00154 void pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov); 00155 00156 // Initialize the filter using some model 00157 void pf_init_model(pf_t *pf, pf_init_model_fn_t init_fn, void *init_data); 00158 00159 // Update the filter with some new action 00160 void pf_update_action(pf_t *pf, pf_action_model_fn_t action_fn, void *action_data); 00161 00162 // Update the filter with some new sensor observation 00163 void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data); 00164 00165 // Resample the distribution 00166 void pf_update_resample(pf_t *pf); 00167 00168 // Compute the CEP statistics (mean and variance). 00169 void pf_get_cep_stats(pf_t *pf, pf_vector_t *mean, double *var); 00170 00171 // Compute the statistics for a particular cluster. Returns 0 if 00172 // there is no such cluster. 00173 int pf_get_cluster_stats(pf_t *pf, int cluster, double *weight, 00174 pf_vector_t *mean, pf_matrix_t *cov); 00175 00176 // Display the sample set 00177 void pf_draw_samples(pf_t *pf, struct _rtk_fig_t *fig, int max_samples); 00178 00179 // Draw the histogram (kdtree) 00180 void pf_draw_hist(pf_t *pf, struct _rtk_fig_t *fig); 00181 00182 // Draw the CEP statistics 00183 void pf_draw_cep_stats(pf_t *pf, struct _rtk_fig_t *fig); 00184 00185 // Draw the cluster statistics 00186 void pf_draw_cluster_stats(pf_t *pf, struct _rtk_fig_t *fig); 00187 00188 /// @endcond 00189 00190 #ifdef __cplusplus 00191 } 00192 #endif 00193 00194 00195 #endif