Fawkes API Fawkes Development Version
pf_pdf.c
1
2/***************************************************************************
3 * pf_pdf.c: Useful pdf functions
4 *
5 * Created: Thu May 24 18:41:18 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: Useful pdf functions
31 * Author: Andrew Howard
32 * Date: 10 Dec 2002
33 *************************************************************************/
34
35#include <assert.h>
36#include <math.h>
37#include <stdlib.h>
38#include <string.h>
39//#include <gsl/gsl_rng.h>
40//#include <gsl/gsl_randist.h>
41
42#include "pf_pdf.h"
43
44/// @cond EXTERNAL
45
46// Random number generator seed value
47static unsigned int pf_pdf_seed;
48
49
50/**************************************************************************
51 * Gaussian
52 *************************************************************************/
53
54// Create a gaussian pdf
55pf_pdf_gaussian_t *pf_pdf_gaussian_alloc(pf_vector_t *x, pf_matrix_t *cx)
56{
57 pf_matrix_t cd;
58 pf_pdf_gaussian_t *pdf;
59
60 pdf = calloc(1, sizeof(pf_pdf_gaussian_t));
61
62 pdf->x = *x;
63 pdf->cx = *cx;
64 //pdf->cxi = pf_matrix_inverse(cx, &pdf->cxdet);
65
66 // Decompose the convariance matrix into a rotation
67 // matrix and a diagonal matrix.
68 pf_matrix_unitary(&pdf->cr, &cd, &pdf->cx);
69 pdf->cd.v[0] = sqrt(cd.m[0][0]);
70 pdf->cd.v[1] = sqrt(cd.m[1][1]);
71 pdf->cd.v[2] = sqrt(cd.m[2][2]);
72
73 // Initialize the random number generator
74 //pdf->rng = gsl_rng_alloc(gsl_rng_taus);
75 //gsl_rng_set(pdf->rng, ++pf_pdf_seed);
76 srand48(++pf_pdf_seed);
77
78 return pdf;
79}
80
81
82// Destroy the pdf
83void pf_pdf_gaussian_free(pf_pdf_gaussian_t *pdf)
84{
85 //gsl_rng_free(pdf->rng);
86 free(pdf);
87 return;
88}
89
90
91/*
92// Compute the value of the pdf at some point [x].
93double pf_pdf_gaussian_value(pf_pdf_gaussian_t *pdf, pf_vector_t x)
94{
95 int i, j;
96 pf_vector_t z;
97 double zz, p;
98
99 z = pf_vector_sub(x, pdf->x);
100
101 zz = 0;
102 for (i = 0; i < 3; i++)
103 for (j = 0; j < 3; j++)
104 zz += z.v[i] * pdf->cxi.m[i][j] * z.v[j];
105
106 p = 1 / (2 * M_PI * pdf->cxdet) * exp(-zz / 2);
107
108 return p;
109}
110*/
111
112
113// Generate a sample from the the pdf.
114pf_vector_t pf_pdf_gaussian_sample(pf_pdf_gaussian_t *pdf)
115{
116 int i, j;
117 pf_vector_t r;
118 pf_vector_t x;
119
120 // Generate a random vector
121 for (i = 0; i < 3; i++)
122 {
123 //r.v[i] = gsl_ran_gaussian(pdf->rng, pdf->cd.v[i]);
124 r.v[i] = pf_ran_gaussian(pdf->cd.v[i]);
125 }
126
127 for (i = 0; i < 3; i++)
128 {
129 x.v[i] = pdf->x.v[i];
130 for (j = 0; j < 3; j++)
131 x.v[i] += pdf->cr.m[i][j] * r.v[j];
132 }
133
134 return x;
135}
136
137// Draw randomly from a zero-mean Gaussian distribution, with standard
138// deviation sigma.
139// We use the polar form of the Box-Muller transformation, explained here:
140// http://www.taygeta.com/random/gaussian.html
141double pf_ran_gaussian(double sigma)
142{
143 double x2, w;
144
145 do
146 {
147 double r;
148 do { r = drand48(); } while (r==0.0);
149 double x1 = 2.0 * r - 1.0;
150 do { r = drand48(); } while (r==0.0);
151 x2 = 2.0 * r - 1.0;
152 w = x1*x1 + x2*x2;
153 } while(w > 1.0 || w==0.0);
154
155 return(sigma * x2 * sqrt(-2.0*log(w)/w));
156}
157
158#if 0
159
160/**************************************************************************
161 * Discrete
162 * Note that GSL v1.3 and earlier contains a bug in the discrete
163 * random generator. A patched version of the the generator is included
164 * in gsl_discrete.c.
165 *************************************************************************/
166
167
168// Create a discrete pdf
169pf_pdf_discrete_t *pf_pdf_discrete_alloc(int count, double *probs)
170{
171 pf_pdf_discrete_t *pdf;
172
173 pdf = calloc(1, sizeof(pf_pdf_discrete_t));
174
175 pdf->prob_count = count;
176 pdf->probs = malloc(count * sizeof(double));
177 memcpy(pdf->probs, probs, count * sizeof(double));
178
179 // Initialize the random number generator
180 pdf->rng = gsl_rng_alloc(gsl_rng_taus);
181 gsl_rng_set(pdf->rng, ++pf_pdf_seed);
182
183 // Initialize the discrete distribution generator
184 pdf->ran = gsl_ran_discrete_preproc(count, probs);
185
186 return pdf;
187}
188
189
190// Destroy the pdf
191void pf_pdf_discrete_free(pf_pdf_discrete_t *pdf)
192{
193 gsl_ran_discrete_free(pdf->ran);
194 gsl_rng_free(pdf->rng);
195 free(pdf->probs);
196 free(pdf);
197 return;
198}
199
200
201// Compute the value of the probability of some element [i]
202double pf_pdf_discrete_value(pf_pdf_discrete_t *pdf, int i)
203{
204 return pdf->probs[i];
205}
206
207
208// Generate a sample from the the pdf.
209int pf_pdf_discrete_sample(pf_pdf_discrete_t *pdf)
210{
211 int i;
212
213 i = gsl_ran_discrete(pdf->rng, pdf->ran);
214 assert(i >= 0 && i < pdf->prob_count);
215
216 return i;
217}
218
219
220#endif
221
222/// @endcond
223