Home
FAQ
Player
Stage
Gazebo
Contrib
Documentation
Publications
Contributors
Users

Project
Download
Bugs/Feedback
Mailing lists

Radish

Old news
Old stuff

pf.h

Go to the documentation of this file.
00001 
00002 /**************************************************************************
00003  * Desc: Simple particle filter for localization.
00004  * Author: Andrew Howard
00005  * Date: 10 Dec 2002
00006  * CVS: $Id: pf.h,v 1.7 2003/11/11 07:20:30 inspectorg Exp $
00007  *************************************************************************/
00008 
00009 #ifndef PF_H
00010 #define PF_H
00011 
00012 #include "pf_vector.h"
00013 #include "pf_kdtree.h"
00014 
00015 #ifdef __cplusplus
00016 extern "C" {
00017 #endif
00018 
00019 // Forward declarations
00020 struct _pf_t;
00021 struct _rtk_fig_t;
00022 
00023 // Function prototype for the initialization model; generates a sample pose from
00024 // an appropriate distribution.
00025 typedef pf_vector_t (*pf_init_model_fn_t) (void *init_data);
00026 
00027 // Function prototype for the action model; generates a sample pose from
00028 // an appropriate distribution
00029 typedef pf_vector_t (*pf_action_model_fn_t) (void *action_data, pf_vector_t pose);
00030 
00031 // Function prototype for the sensor model; determines the probability
00032 // for the given sample pose.
00033 typedef double (*pf_sensor_model_fn_t) (void *sensor_data, pf_vector_t pose);
00034 
00035 
00036 // Information for a single sample
00037 typedef struct
00038 {
00039   // Pose represented by this sample
00040   pf_vector_t pose;
00041 
00042   // Weight for this pose
00043   double weight;
00044   
00045 } pf_sample_t;
00046 
00047 
00048 // Information for a cluster of samples
00049 typedef struct
00050 {
00051   // Number of samples
00052   int count;
00053 
00054   // Total weight of samples in this cluster
00055   double weight;
00056 
00057   // Cluster statistics
00058   pf_vector_t mean;
00059   pf_matrix_t cov;
00060 
00061   // Workspace
00062   double m[4], c[2][2];
00063   
00064 } pf_cluster_t;
00065 
00066 
00067 // Information for a set of samples
00068 typedef struct
00069 {
00070   // The samples
00071   int sample_count;
00072   pf_sample_t *samples;
00073 
00074   // A kdtree encoding the histogram
00075   pf_kdtree_t *kdtree;
00076 
00077   // Clusters
00078   int cluster_count, cluster_max_count;
00079   pf_cluster_t *clusters;
00080   
00081 } pf_sample_set_t;
00082 
00083 
00084 // Information for an entire filter
00085 typedef struct _pf_t
00086 {
00087   // This min and max number of samples
00088   int min_samples, max_samples;
00089 
00090   // Population size parameters
00091   double pop_err, pop_z;
00092   
00093   // The sample sets.  We keep two sets and use [current_set]
00094   // to identify the active set.
00095   int current_set;
00096   pf_sample_set_t sets[2];
00097 
00098 } pf_t;
00099 
00100 
00101 // Create a new filter
00102 pf_t *pf_alloc(int min_samples, int max_samples);
00103 
00104 // Free an existing filter
00105 void pf_free(pf_t *pf);
00106 
00107 // Initialize the filter using a guassian
00108 void pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov);
00109 
00110 // Initialize the filter using some model
00111 void pf_init_model(pf_t *pf, pf_init_model_fn_t init_fn, void *init_data);
00112 
00113 // Update the filter with some new action
00114 void pf_update_action(pf_t *pf, pf_action_model_fn_t action_fn, void *action_data);
00115 
00116 // Update the filter with some new sensor observation
00117 void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data);
00118 
00119 // Resample the distribution
00120 void pf_update_resample(pf_t *pf);
00121 
00122 // Compute the CEP statistics (mean and variance).
00123 void pf_get_cep_stats(pf_t *pf, pf_vector_t *mean, double *var);
00124 
00125 // Compute the statistics for a particular cluster.  Returns 0 if
00126 // there is no such cluster.
00127 int pf_get_cluster_stats(pf_t *pf, int cluster, double *weight,
00128                          pf_vector_t *mean, pf_matrix_t *cov);
00129 
00130 // Display the sample set
00131 void pf_draw_samples(pf_t *pf, struct _rtk_fig_t *fig, int max_samples);
00132 
00133 // Draw the histogram (kdtree)
00134 void pf_draw_hist(pf_t *pf, struct _rtk_fig_t *fig);
00135 
00136 // Draw the CEP statistics
00137 void pf_draw_cep_stats(pf_t *pf, struct _rtk_fig_t *fig);
00138 
00139 // Draw the cluster statistics
00140 void pf_draw_cluster_stats(pf_t *pf, struct _rtk_fig_t *fig);
00141 
00142 #ifdef __cplusplus
00143 }
00144 #endif
00145 
00146 
00147 #endif

Generated on Tue May 3 14:15:35 2005 for Player by doxygen 1.3.6