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
42extern "C" {
43#endif
44
45/// @cond EXTERNAL
46
47// Forward declarations
48struct _pf_t;
49struct _rtk_fig_t;
50struct _pf_sample_set_t;
51
52// Function prototype for the initialization model; generates a sample pose from
53// an appropriate distribution.
54typedef 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
58typedef 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.
62typedef double (*pf_sensor_model_fn_t)(void *sensor_data, struct _pf_sample_set_t *set);
63
64// Information for a single sample
65typedef 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
76typedef 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
94typedef 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
114typedef 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
139pf_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
147void pf_free(pf_t *pf);
148
149// Initialize the filter using a guassian
150void pf_init(pf_t *pf, pf_vector_t *mean, pf_matrix_t *cov);
151
152// Initialize the filter using some model
153void 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
156void 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
159void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data);
160
161// Resample the distribution
162void pf_update_resample(pf_t *pf);
163
164// Compute the CEP statistics (mean and variance).
165void 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.
169int
170pf_get_cluster_stats(pf_t *pf, int cluster, double *weight, pf_vector_t *mean, pf_matrix_t *cov);
171
172// Display the sample set
173void pf_draw_samples(pf_t *pf, struct _rtk_fig_t *fig, int max_samples);
174
175// Draw the histogram (kdtree)
176void pf_draw_hist(pf_t *pf, struct _rtk_fig_t *fig);
177
178// Draw the CEP statistics
179void pf_draw_cep_stats(pf_t *pf, struct _rtk_fig_t *fig);
180
181// Draw the cluster statistics
182void 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