Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include <assert.h>
00029 #include <math.h>
00030 #include <stdlib.h>
00031 #include <string.h>
00032
00033
00034
00035 #include "nav2d_localizer/pf_pdf.h"
00036
00037
00038 static unsigned int pf_pdf_seed;
00039
00040
00041
00042
00043
00044
00045
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
00056
00057
00058
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
00065
00066
00067 srand48(++pf_pdf_seed);
00068
00069 return pdf;
00070 }
00071
00072
00073
00074 void pf_pdf_gaussian_free(pf_pdf_gaussian_t *pdf)
00075 {
00076
00077 free(pdf);
00078 return;
00079 }
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
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
00112 for (i = 0; i < 3; i++)
00113 {
00114
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
00129
00130
00131
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
00152
00153
00154
00155
00156
00157
00158
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
00170 pdf->rng = gsl_rng_alloc(gsl_rng_taus);
00171 gsl_rng_set(pdf->rng, ++pf_pdf_seed);
00172
00173
00174 pdf->ran = gsl_ran_discrete_preproc(count, probs);
00175
00176 return pdf;
00177 }
00178
00179
00180
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
00192 double pf_pdf_discrete_value(pf_pdf_discrete_t *pdf, int i)
00193 {
00194 return pdf->probs[i];
00195 }
00196
00197
00198
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