Fawkes API  Fawkes Development Version
pf.h
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