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


amcl
Author(s): Brian P. Gerkey, contradict@gmail.com
autogenerated on Thu Aug 27 2015 14:07:48