$search
00001 /* 00002 * Player - One Hell of a Robot Server 00003 * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 00004 * gerkey@usc.edu kaspers@robotics.usc.edu 00005 * 00006 * This library is free software; you can redistribute it and/or 00007 * modify it under the terms of the GNU Lesser General Public 00008 * License as published by the Free Software Foundation; either 00009 * version 2.1 of the License, or (at your option) any later version. 00010 * 00011 * This library is distributed in the hope that it will be useful, 00012 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00013 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 00014 * Lesser General Public License for more details. 00015 * 00016 * You should have received a copy of the GNU Lesser General Public 00017 * License along with this library; if not, write to the Free Software 00018 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 00019 * 00020 */ 00021 /************************************************************************** 00022 * Desc: Useful pdf functions 00023 * Author: Andrew Howard 00024 * Date: 10 Dec 2002 00025 * CVS: $Id: pf_pdf.c 6348 2008-04-17 02:53:17Z gerkey $ 00026 *************************************************************************/ 00027 00028 #include <assert.h> 00029 #include <math.h> 00030 #include <stdlib.h> 00031 #include <string.h> 00032 //#include <gsl/gsl_rng.h> 00033 //#include <gsl/gsl_randist.h> 00034 00035 #include "pf_pdf.h" 00036 00037 // Random number generator seed value 00038 static unsigned int pf_pdf_seed; 00039 00040 00041 /************************************************************************** 00042 * Gaussian 00043 *************************************************************************/ 00044 00045 // Create a gaussian pdf 00046 pf_pdf_gaussian_t *pf_pdf_gaussian_alloc(pf_vector_t x, pf_matrix_t cx) 00047 { 00048 pf_matrix_t cd; 00049 pf_pdf_gaussian_t *pdf; 00050 00051 pdf = calloc(1, sizeof(pf_pdf_gaussian_t)); 00052 00053 pdf->x = x; 00054 pdf->cx = cx; 00055 //pdf->cxi = pf_matrix_inverse(cx, &pdf->cxdet); 00056 00057 // Decompose the convariance matrix into a rotation 00058 // matrix and a diagonal matrix. 00059 pf_matrix_unitary(&pdf->cr, &cd, pdf->cx); 00060 pdf->cd.v[0] = sqrt(cd.m[0][0]); 00061 pdf->cd.v[1] = sqrt(cd.m[1][1]); 00062 pdf->cd.v[2] = sqrt(cd.m[2][2]); 00063 00064 // Initialize the random number generator 00065 //pdf->rng = gsl_rng_alloc(gsl_rng_taus); 00066 //gsl_rng_set(pdf->rng, ++pf_pdf_seed); 00067 srand48(++pf_pdf_seed); 00068 00069 return pdf; 00070 } 00071 00072 00073 // Destroy the pdf 00074 void pf_pdf_gaussian_free(pf_pdf_gaussian_t *pdf) 00075 { 00076 //gsl_rng_free(pdf->rng); 00077 free(pdf); 00078 return; 00079 } 00080 00081 00082 /* 00083 // Compute the value of the pdf at some point [x]. 00084 double pf_pdf_gaussian_value(pf_pdf_gaussian_t *pdf, pf_vector_t x) 00085 { 00086 int i, j; 00087 pf_vector_t z; 00088 double zz, p; 00089 00090 z = pf_vector_sub(x, pdf->x); 00091 00092 zz = 0; 00093 for (i = 0; i < 3; i++) 00094 for (j = 0; j < 3; j++) 00095 zz += z.v[i] * pdf->cxi.m[i][j] * z.v[j]; 00096 00097 p = 1 / (2 * M_PI * pdf->cxdet) * exp(-zz / 2); 00098 00099 return p; 00100 } 00101 */ 00102 00103 00104 // Generate a sample from the the pdf. 00105 pf_vector_t pf_pdf_gaussian_sample(pf_pdf_gaussian_t *pdf) 00106 { 00107 int i, j; 00108 pf_vector_t r; 00109 pf_vector_t x; 00110 00111 // Generate a random vector 00112 for (i = 0; i < 3; i++) 00113 { 00114 //r.v[i] = gsl_ran_gaussian(pdf->rng, pdf->cd.v[i]); 00115 r.v[i] = pf_ran_gaussian(pdf->cd.v[i]); 00116 } 00117 00118 for (i = 0; i < 3; i++) 00119 { 00120 x.v[i] = pdf->x.v[i]; 00121 for (j = 0; j < 3; j++) 00122 x.v[i] += pdf->cr.m[i][j] * r.v[j]; 00123 } 00124 00125 return x; 00126 } 00127 00128 // Draw randomly from a zero-mean Gaussian distribution, with standard 00129 // deviation sigma. 00130 // We use the polar form of the Box-Muller transformation, explained here: 00131 // http://www.taygeta.com/random/gaussian.html 00132 double pf_ran_gaussian(double sigma) 00133 { 00134 double x1, x2, w, r; 00135 00136 do 00137 { 00138 do { r = drand48(); } while (r==0.0); 00139 x1 = 2.0 * r - 1.0; 00140 do { r = drand48(); } while (r==0.0); 00141 x2 = 2.0 * r - 1.0; 00142 w = x1*x1 + x2*x2; 00143 } while(w > 1.0 || w==0.0); 00144 00145 return(sigma * x2 * sqrt(-2.0*log(w)/w)); 00146 } 00147 00148 #if 0 00149 00150 /************************************************************************** 00151 * Discrete 00152 * Note that GSL v1.3 and earlier contains a bug in the discrete 00153 * random generator. A patched version of the the generator is included 00154 * in gsl_discrete.c. 00155 *************************************************************************/ 00156 00157 00158 // Create a discrete pdf 00159 pf_pdf_discrete_t *pf_pdf_discrete_alloc(int count, double *probs) 00160 { 00161 pf_pdf_discrete_t *pdf; 00162 00163 pdf = calloc(1, sizeof(pf_pdf_discrete_t)); 00164 00165 pdf->prob_count = count; 00166 pdf->probs = malloc(count * sizeof(double)); 00167 memcpy(pdf->probs, probs, count * sizeof(double)); 00168 00169 // Initialize the random number generator 00170 pdf->rng = gsl_rng_alloc(gsl_rng_taus); 00171 gsl_rng_set(pdf->rng, ++pf_pdf_seed); 00172 00173 // Initialize the discrete distribution generator 00174 pdf->ran = gsl_ran_discrete_preproc(count, probs); 00175 00176 return pdf; 00177 } 00178 00179 00180 // Destroy the pdf 00181 void pf_pdf_discrete_free(pf_pdf_discrete_t *pdf) 00182 { 00183 gsl_ran_discrete_free(pdf->ran); 00184 gsl_rng_free(pdf->rng); 00185 free(pdf->probs); 00186 free(pdf); 00187 return; 00188 } 00189 00190 00191 // Compute the value of the probability of some element [i] 00192 double pf_pdf_discrete_value(pf_pdf_discrete_t *pdf, int i) 00193 { 00194 return pdf->probs[i]; 00195 } 00196 00197 00198 // Generate a sample from the the pdf. 00199 int pf_pdf_discrete_sample(pf_pdf_discrete_t *pdf) 00200 { 00201 int i; 00202 00203 i = gsl_ran_discrete(pdf->rng, pdf->ran); 00204 assert(i >= 0 && i < pdf->prob_count); 00205 00206 return i; 00207 } 00208 00209 #endif