Fawkes API  Fawkes Development Version
pf.h
1 
2 /***************************************************************************
3  * pf.h: Simple particle filter for localization
4  *
5  * Created: Wed May 16 16:04:41 2012
6  * Copyright 2000 Brian Gerkey
7  * 2000 Kasper Stoy
8  * 2012 Tim Niemueller [www.niemueller.de]
9  ****************************************************************************/
10 
11 /* This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL file in the doc directory.
22  */
23 
24 /* From:
25  * Player - One Hell of a Robot Server (LGPL)
26  * Copyright (C) 2000 Brian Gerkey & Kasper Stoy
27  * gerkey@usc.edu kaspers@robotics.usc.edu
28  */
29 /**************************************************************************
30  * Desc: Simple particle filter for localization.
31  * Author: Andrew Howard
32  * Date: 10 Dec 2002
33  *************************************************************************/
34 
35 #ifndef PF_H
36 #define PF_H
37 
38 #include "pf_vector.h"
39 #include "pf_kdtree.h"
40 
41 #ifdef __cplusplus
42 extern "C" {
43 #endif
44 
45 /// @cond EXTERNAL
46 
47 // Forward declarations
48 struct _pf_t;
49 struct _rtk_fig_t;
50 struct _pf_sample_set_t;
51 
52 // Function prototype for the initialization model; generates a sample pose from
53 // an appropriate distribution.
54 typedef pf_vector_t (*pf_init_model_fn_t) (void *init_data);
55 
56 // Function prototype for the action model; generates a sample pose from
57 // an appropriate distribution
58 typedef void (*pf_action_model_fn_t) (void *action_data,
59  struct _pf_sample_set_t* set);
60 
61 // Function prototype for the sensor model; determines the probability
62 // for the given set of sample poses.
63 typedef double (*pf_sensor_model_fn_t) (void *sensor_data,
64  struct _pf_sample_set_t* set);
65 
66 
67 // Information for a single sample
68 typedef struct
69 {
70  // Pose represented by this sample
71  pf_vector_t pose;
72 
73  // Weight for this pose
74  double weight;
75 
76 } pf_sample_t;
77 
78 
79 // Information for a cluster of samples
80 typedef struct
81 {
82  // Number of samples
83  int count;
84 
85  // Total weight of samples in this cluster
86  double weight;
87 
88  // Cluster statistics
89  pf_vector_t mean;
90  pf_matrix_t cov;
91 
92  // Workspace
93  double m[4], c[2][2];
94 
95 } pf_cluster_t;
96 
97 
98 // Information for a set of samples
99 typedef struct _pf_sample_set_t
100 {
101  // The samples
102  int sample_count;
103  pf_sample_t *samples;
104 
105  // A kdtree encoding the histogram
106  pf_kdtree_t *kdtree;
107 
108  // Clusters
109  int cluster_count, cluster_max_count;
110  pf_cluster_t *clusters;
111 
112  // Filter statistics
113  pf_vector_t mean;
114  pf_matrix_t cov;
115 
116 } pf_sample_set_t;
117 
118 
119 // Information for an entire filter
120 typedef struct _pf_t
121 {
122  // This min and max number of samples
123  int min_samples, max_samples;
124 
125  // Population size parameters
126  double pop_err, pop_z;
127 
128  // The sample sets. We keep two sets and use [current_set]
129  // to identify the active set.
130  int current_set;
131  pf_sample_set_t sets[2];
132 
133  // Running averages, slow and fast, of likelihood
134  double w_slow, w_fast;
135 
136  // Decay rates for running averages
137  double alpha_slow, alpha_fast;
138 
139  // Function used to draw random pose samples
140  pf_init_model_fn_t random_pose_fn;
141  void *random_pose_data;
142 } pf_t;
143 
144 
145 // Create a new filter
146 pf_t *pf_alloc(int min_samples, int max_samples,
147  double alpha_slow, double alpha_fast,
148  pf_init_model_fn_t random_pose_fn, void *random_pose_data);
149 
150 // Free an existing filter
151 void pf_free(pf_t *pf);
152 
153 // Initialize the filter using a guassian
154 void pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov);
155 
156 // Initialize the filter using some model
157 void pf_init_model(pf_t *pf, pf_init_model_fn_t init_fn, void *init_data);
158 
159 // Update the filter with some new action
160 void pf_update_action(pf_t *pf, pf_action_model_fn_t action_fn, void *action_data);
161 
162 // Update the filter with some new sensor observation
163 void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data);
164 
165 // Resample the distribution
166 void pf_update_resample(pf_t *pf);
167 
168 // Compute the CEP statistics (mean and variance).
169 void pf_get_cep_stats(pf_t *pf, pf_vector_t *mean, double *var);
170 
171 // Compute the statistics for a particular cluster. Returns 0 if
172 // there is no such cluster.
173 int pf_get_cluster_stats(pf_t *pf, int cluster, double *weight,
174  pf_vector_t *mean, pf_matrix_t *cov);
175 
176 // Display the sample set
177 void pf_draw_samples(pf_t *pf, struct _rtk_fig_t *fig, int max_samples);
178 
179 // Draw the histogram (kdtree)
180 void pf_draw_hist(pf_t *pf, struct _rtk_fig_t *fig);
181 
182 // Draw the CEP statistics
183 void pf_draw_cep_stats(pf_t *pf, struct _rtk_fig_t *fig);
184 
185 // Draw the cluster statistics
186 void pf_draw_cluster_stats(pf_t *pf, struct _rtk_fig_t *fig);
187 
188 /// @endcond
189 
190 #ifdef __cplusplus
191 }
192 #endif
193 
194 
195 #endif