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_kdtree.h"
39 #include "pf_vector.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, struct _pf_sample_set_t *set);
59 
60 // Function prototype for the sensor model; determines the probability
61 // for the given set of sample poses.
62 typedef double (*pf_sensor_model_fn_t)(void *sensor_data, struct _pf_sample_set_t *set);
63 
64 // Information for a single sample
65 typedef struct
66 {
67  // Pose represented by this sample
68  pf_vector_t pose;
69 
70  // Weight for this pose
71  double weight;
72 
73 } pf_sample_t;
74 
75 // Information for a cluster of samples
76 typedef struct
77 {
78  // Number of samples
79  int count;
80 
81  // Total weight of samples in this cluster
82  double weight;
83 
84  // Cluster statistics
85  pf_vector_t mean;
86  pf_matrix_t cov;
87 
88  // Workspace
89  double m[4], c[2][2];
90 
91 } pf_cluster_t;
92 
93 // Information for a set of samples
94 typedef struct _pf_sample_set_t
95 {
96  // The samples
97  int sample_count;
98  pf_sample_t *samples;
99 
100  // A kdtree encoding the histogram
101  pf_kdtree_t *kdtree;
102 
103  // Clusters
104  int cluster_count, cluster_max_count;
105  pf_cluster_t *clusters;
106 
107  // Filter statistics
108  pf_vector_t mean;
109  pf_matrix_t cov;
110 
111 } pf_sample_set_t;
112 
113 // Information for an entire filter
114 typedef struct _pf_t
115 {
116  // This min and max number of samples
117  int min_samples, max_samples;
118 
119  // Population size parameters
120  double pop_err, pop_z;
121 
122  // The sample sets. We keep two sets and use [current_set]
123  // to identify the active set.
124  int current_set;
125  pf_sample_set_t sets[2];
126 
127  // Running averages, slow and fast, of likelihood
128  double w_slow, w_fast;
129 
130  // Decay rates for running averages
131  double alpha_slow, alpha_fast;
132 
133  // Function used to draw random pose samples
134  pf_init_model_fn_t random_pose_fn;
135  void * random_pose_data;
136 } pf_t;
137 
138 // Create a new filter
139 pf_t *pf_alloc(int min_samples,
140  int max_samples,
141  double alpha_slow,
142  double alpha_fast,
143  pf_init_model_fn_t random_pose_fn,
144  void * random_pose_data);
145 
146 // Free an existing filter
147 void pf_free(pf_t *pf);
148 
149 // Initialize the filter using a guassian
150 void pf_init(pf_t *pf, pf_vector_t *mean, pf_matrix_t *cov);
151 
152 // Initialize the filter using some model
153 void pf_init_model(pf_t *pf, pf_init_model_fn_t init_fn, void *init_data);
154 
155 // Update the filter with some new action
156 void pf_update_action(pf_t *pf, pf_action_model_fn_t action_fn, void *action_data);
157 
158 // Update the filter with some new sensor observation
159 void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data);
160 
161 // Resample the distribution
162 void pf_update_resample(pf_t *pf);
163 
164 // Compute the CEP statistics (mean and variance).
165 void pf_get_cep_stats(pf_t *pf, pf_vector_t *mean, double *var);
166 
167 // Compute the statistics for a particular cluster. Returns 0 if
168 // there is no such cluster.
169 int
170 pf_get_cluster_stats(pf_t *pf, int cluster, double *weight, pf_vector_t *mean, pf_matrix_t *cov);
171 
172 // Display the sample set
173 void pf_draw_samples(pf_t *pf, struct _rtk_fig_t *fig, int max_samples);
174 
175 // Draw the histogram (kdtree)
176 void pf_draw_hist(pf_t *pf, struct _rtk_fig_t *fig);
177 
178 // Draw the CEP statistics
179 void pf_draw_cep_stats(pf_t *pf, struct _rtk_fig_t *fig);
180 
181 // Draw the cluster statistics
182 void pf_draw_cluster_stats(pf_t *pf, struct _rtk_fig_t *fig);
183 
184 /// @endcond
185 
186 #ifdef __cplusplus
187 }
188 #endif
189 
190 #endif