pf.h
1/*
2 * Player - One Hell of a Robot Server
3 * Copyright (C) 2003
4 * Andrew Howard
5 * Brian Gerkey
6 *
7 * This program is free software; you can redistribute it and/or modify
8 * it under the terms of the GNU General Public License as published by
9 * the Free Software Foundation; either version 2 of the License, or
10 * (at your option) any later version.
11 *
12 * This program is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 * GNU General Public License for more details.
16 *
17 * You should have received a copy of the GNU General Public License
18 * along with this program; if not, write to the Free Software
19 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
20 *
21 */
22
23
24/**************************************************************************
25 * Desc: Simple particle filter for localization.
26 * Author: Andrew Howard
27 * Date: 10 Dec 2002
28 * CVS: $Id$
29 *************************************************************************/
30
31#ifndef PF_H
32#define PF_H
33
34#include "pf_vector.h"
35#include "pf_kdtree.h"
36
37#ifdef __cplusplus
38extern "C" {
39#endif
40
41// Forward declarations
42struct _pf_t;
43struct _rtk_fig_t;
44struct _pf_sample_set_t;
45
46// Function prototype for the initialization model; generates a sample pose from
47// an appropriate distribution.
48typedef pf_vector_t (*pf_init_model_fn_t) (void *init_data);
49
50// Function prototype for the action model; generates a sample pose from
51// an appropriate distribution
52typedef void (*pf_action_model_fn_t) (void *action_data,
53 struct _pf_sample_set_t* set);
54
55// Function prototype for the sensor model; determines the probability
56// for the given set of sample poses.
57typedef double (*pf_sensor_model_fn_t) (void *sensor_data,
58 struct _pf_sample_set_t* set);
59
60
61// Information for a single sample
62typedef struct
63{
64 // Pose represented by this sample
65 pf_vector_t pose;
66
67 // Weight for this pose
68 double weight;
69
71
72
73// Information for a cluster of samples
74typedef struct
75{
76 // Number of samples
77 int count;
78
79 // Total weight of samples in this cluster
80 double weight;
81
82 // Cluster statistics
83 pf_vector_t mean;
84 pf_matrix_t cov;
85
86 // Workspace
87 double m[4], c[2][2];
88
90
91
92// Information for a set of samples
93typedef struct _pf_sample_set_t
94{
95 // The samples
96 int sample_count;
97 pf_sample_t *samples;
98
99 // A kdtree encoding the histogram
100 pf_kdtree_t *kdtree;
101
102 // Clusters
103 int cluster_count, cluster_max_count;
104 pf_cluster_t *clusters;
105
107
108
109// Information for an entire filter
110typedef struct _pf_t
111{
112 // This min and max number of samples
113 int min_samples, max_samples;
114
115 // Population size parameters
116 double pop_err, pop_z;
117
118 // The sample sets. We keep two sets and use [current_set]
119 // to identify the active set.
120 int current_set;
121 pf_sample_set_t sets[2];
122
123} pf_t;
124
125
126// Create a new filter
127pf_t *pf_alloc(int min_samples, int max_samples);
128
129// Free an existing filter
130void pf_free(pf_t *pf);
131
132// Initialize the filter using a guassian
133void pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov);
134
135// Initialize the filter using some model
136void pf_init_model(pf_t *pf, pf_init_model_fn_t init_fn, void *init_data);
137
138// Update the filter with some new action
139void pf_update_action(pf_t *pf, pf_action_model_fn_t action_fn, void *action_data);
140
141// Update the filter with some new sensor observation
142void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data);
143
144// Resample the distribution
145void pf_update_resample(pf_t *pf);
146
147// Compute the CEP statistics (mean and variance).
148void pf_get_cep_stats(pf_t *pf, pf_vector_t *mean, double *var);
149
150// Compute the statistics for a particular cluster. Returns 0 if
151// there is no such cluster.
152int pf_get_cluster_stats(pf_t *pf, int cluster, double *weight,
153 pf_vector_t *mean, pf_matrix_t *cov);
154
155// Display the sample set
156void pf_draw_samples(pf_t *pf, struct _rtk_fig_t *fig, int max_samples);
157
158// Draw the histogram (kdtree)
159void pf_draw_hist(pf_t *pf, struct _rtk_fig_t *fig);
160
161// Draw the CEP statistics
162void pf_draw_cep_stats(pf_t *pf, struct _rtk_fig_t *fig);
163
164// Draw the cluster statistics
165void pf_draw_cluster_stats(pf_t *pf, struct _rtk_fig_t *fig);
166
167#ifdef __cplusplus
168}
169#endif
170
171
172#endif
Definition pf.h:94
Definition pf.h:111
Definition pf.h:75
Definition pf_kdtree.h:68
Definition pf_vector.h:49
Definition pf.h:63
Definition pf_vector.h:42