/* pf.c * written by Andrew Stout, Oliver Hsu * 10/19/02 */ /* * Okay, so hopefully this will become our particle filter. * We want it to be as general as possible, so we can use it for both * vision and localization purposes. This implies modularity and abstraction. * --Andrew Stout 10-19-02 */ /* * thoughts 11/27 * splitting pf_update into pf_predict and pf_correct. We want to be able to * run pf_predict several times before we run pf_correct: when correction data * is only available at a low rate, we don't want to update predict once and * add a lot of noise. Instead, we want to predict whenever we have data to * predict with (odometry), adding a little bit of noise each time. */ /* Example: * this is an example of how to use this particle filter library for tracking * * It assumes that the position field in the particle struct is to be linked * to the following struct: * typedef struct { * int x; * int y; * } ex_pos; * * The init_particle function might then look like this: * void ex_init_particle(void *ex_pdmap, void *position){ * */ /* * we need: * - a particle datatype * - position * - weight * - other info? * - an array of particles * * then we need: * - Initialization * loop: (is this locked to some sensing rate?) * - Prediction * - prediction rule for position: function of prev. pos * - prediction rule for weight: exactly prev. weight * - Correction * - correction rule for position: exactly predicted position * - correction rule for weight: probability of obs. given pred. times weight * - Resampling * - if variance too big * - draw N samples from samples, proportional to weight (with replacement) * */ #include #include #include //for random seed #include "pf.h" #include #include /* this function initializes the particle filter (not to be confused with the * initialization step of the actual particle filter algorithm, which is done * in this function, but this function has a larger purpose of also * allocating and initializing the data structures associated with the * particle filter). * - It takes: * - N, the number of particles * - a pointer to the initial data * - the size of the position struct to be linked to a particle (as * returned by sizeof()) * - q is the proportion of particles to be initialized each iteration [0, 1] * - r is the proportion of particles to be sampled from the importance function each iteration [0, 1] * - init_particle, a pointer to a function which takes the initial data * and a pointer to the position, and sets (probablistically) the position * and weight of that particle according to initial data * - a pointer to a function to print position data, takes a pointer to the * position * - a pointer to a function to predict the position of a particle at the * next time step, takes a pointer to the position * - a pointer to a function to calculate the probability of getting a * particular sensor reading, assuming that we are at some particular * position, takes a pointer to the sensor data and a pointer to the * position in question * - a pointer to a function to perform importance sampling, takes pointer to * sensor data already tailored towards importance sampling and a pointer * to the position to be changed * - a pointer to a function to calculate probability of one position being * the successor of another in successive time steps, takes two pointers to * positions * - a pointer to a function to copy position data, takes two pointers to * positions, target and source * - a pointer to a function to compute a weighted sum of positions, takes * a pointer to an accumulating position, a pointer to a position to be * added, and a weight * - a pointer to a function to free position data, takes a pointer to a * position * * The function allocates a new particle filter, * sets the parameters of the particle filter (variables, function pointers), * allocates memory for the particle filter, and calls init_particle on each * particle to initialize the particle filter, [perhaps more to come]. * */ static int * pos_addr, *pos_buffer_addr; particle_filter *pf_init(int num_particles, void *data, int sizeof_position, double q, double r, int ignore_old_weight, double (*init_particle)(void *, void *), void (*predict_pos)(void *, void *), double (*prob_pos)(void *, void*), double (*icond_sample_func)(void *, void *), double (*icond_pos_likelihood_func)(void *, void *), void (*copy_pos)(void *, void*), void (*zero_pos)(void *), void (*sum_pos)(void *, void *, double), void (*print_pos)(void *), void (*free_pos)(void *)) { particle_filter *pf; int n; double wsum, cur_cum_weight; pf = (particle_filter *)malloc(sizeof(particle_filter)); if (pf == NULL) { fprintf(stderr, "memory allocation error\n"); exit(1); } memset(pf, 0, sizeof(particle_filter)); pf->N = num_particles; pf->particles = (particle *)malloc(pf->N * sizeof(particle)); pf->buffer = (particle *)malloc(pf->N * sizeof(particle)); if (pf->particles == NULL || pf->buffer == NULL) { printf("memory allocation error\n"); exit(1); } memset(pf->particles, 0, pf->N * sizeof(particle)); memset(pf->buffer, 0, pf->N * sizeof(particle)); pf->positions=malloc(sizeof_position*num_particles); pf->buffer_positions=malloc(sizeof_position*num_particles); if (!pf->positions || !pf->buffer_positions) { fprintf(stderr, "memory allocation error\n"); exit(1); } memset(pf->positions, 0, pf->N * sizeof_position); memset(pf->buffer_positions, 0, pf->N * sizeof_position); pos_addr = (int *)malloc(pf->N * sizeof(int)); pos_buffer_addr = (int *)malloc(pf->N * sizeof(int)); if (!pos_addr|| !pos_buffer_addr) { fprintf(stderr, "memory allocation error\n"); exit(1); } memset(pos_addr, 0, pf->N * sizeof(int)); memset(pos_buffer_addr, 0, pf->N * sizeof(int)); wsum = 0.0; for(n=0; nN; n++){ pf->particles[n].position = pf->positions+(sizeof_position*n); pf->buffer[n].position = pf->buffer_positions+(sizeof_position*n); pos_addr[n] = (int)pf->particles[n].position; pos_buffer_addr[n] = (int)pf->buffer[n].position; pf->particles[n].weight = (*init_particle)(data, pf->particles[n].position); /* Initialization step of PF * algorithm */ #if DEBUG printf("particle %d initial weight %lf\n", n, pf->particles[n].weight); #endif //pf->particles[n].method = INIT; wsum += pf->particles[n].weight; } /* normalize weights, set cumulative weights */ cur_cum_weight = 0; for(n=0; nN; n++) { if (wsum) pf->particles[n].weight = pf->particles[n].weight / wsum; else pf->particles[n].weight = (double)1/(double)pf->N; cur_cum_weight += pf->particles[n].weight; pf->particles[n].cum_weight = cur_cum_weight; #if DEBUG printf("particle %d cum_weight = %g\n", n, pf->particles[n].cum_weight); #endif } pf->q = q; pf->r = r; pf->ignore_old_weight = ignore_old_weight; pf->pos_size = sizeof_position; // initialize random numbers srand(time(NULL)); // set function pointers pf->print_pos = print_pos; pf->predict_pos = predict_pos; pf->prob_pos = prob_pos; pf->icond_sample_func = icond_sample_func; pf->icond_pos_likelihood_func= icond_pos_likelihood_func; pf->copy_pos = copy_pos; pf->sum_pos = sum_pos; pf->zero_pos = zero_pos; pf->free_pos = free_pos; return pf; } /* employs roulette wheel selection, seeding a new generation of particles * before: parent generation in particles array * after: new generation in particles array, parent generation in buffer array * weights are normalized. */ int pf_resample(particle_filter *pf) { int i, m, jump; double randVal, sumWeight = 0, cur_cum_weight = 0; particle *temp; void *temp_pos; for (i = 0; i < pf->N; i++) { randVal = rand()/(RAND_MAX+1.0); m = (pf->N) / 2; jump = (pf->N) / 2; while(m >= 0 && m < (pf->N)-1) { if(m == 0 && pf->particles[m].cum_weight >= randVal) { break; } if(m == pf->N - 1 && pf->particles[m].cum_weight < randVal) { break; } if(pf->particles[m-1].cum_weight < randVal && pf->particles[m].cum_weight >= randVal) { break; } if (jump > 1) { jump /= 2; } if (pf->particles[m-1].cum_weight < randVal) { m += jump; } else { m -= jump; } } #if DEBUG printf("m = %d\n", m); #endif (*(pf->copy_pos))(pf->buffer[i].position, pf->particles[m].position); pf->buffer[i].weight = pf->particles[m].weight; // pf->buffer[i].cum_weight= pf->particles[m].cum_weight; sumWeight += pf->buffer[i].weight; } #if DEBUG printf("normalizing weights...\n"); #endif for (i = 0; i < pf->N; i++) { if (sumWeight) pf->buffer[i].weight /= sumWeight; else pf->buffer[i].weight = (double)1/(double)pf->N; cur_cum_weight += pf->buffer[i].weight; pf->buffer[i].cum_weight = cur_cum_weight; #if DEBUG printf("particle %d cum_weight = %g\n", n, pf->buffer[i].cum_weight); #endif } temp = pf->particles; pf->particles = pf->buffer; pf->buffer = temp; temp_pos = pf->positions; pf->positions = pf->buffer_positions; pf->buffer_positions = temp_pos; // buffer now holds previous generation return 0; } /* apply motion model to particles */ int pf_predict(particle_filter *pf, void *predict_data) { int i; for (i = 0; i < pf->N; i++) { (*(pf->predict_pos))(predict_data,pf->particles[i].position); } return 0; } /* picks a sampling method and updates particles accordingly */ int pf_correct(particle_filter *pf, void *correct_data, void *importance_data) { int i, m; double meth; double sum_icond_pos_likelihood= 0; double sumWeight = 0, cur_cum_weight = 0; sampling_method method; for (i = 0; i < pf->N; i++) { meth = rand()/(RAND_MAX+1.0); if (meth < pf->q) method = INIT; else if (meth < (pf->q)+(pf->r)) method = IMPORTANCE; else method = CONDENSATION; switch(method){ case INIT: (*(pf->icond_sample_func)) (importance_data, pf->particles[i].position); pf->particles[i].weight = (*(pf->prob_pos)) (correct_data,pf->particles[i].position); break; case IMPORTANCE: pf->particles[i].weight = 1 / (*(pf->icond_sample_func)) (importance_data, pf->particles[i].position); sum_icond_pos_likelihood= 0; for (m = 0; m < pf->N; m++) { // buffer holds previous generation sum_icond_pos_likelihood += pf->particles[m].weight * (*(pf->icond_pos_likelihood_func))(pf->particles[i].position, pf->buffer[m].position); } pf->particles[i].weight = sum_icond_pos_likelihood * (*(pf->prob_pos))(correct_data,pf->particles[i].position) * pf->particles[i].weight; break; case CONDENSATION: pf->particles[i].weight = (*(pf->prob_pos)) (correct_data,pf->particles[i].position) * ((pf->ignore_old_weight)? 1 : pf->particles[i].weight); #if DEBUG printf("particle %d gets weight %.24lf\n", i, pf->particles[i].weight); #endif } sumWeight += pf->particles[i].weight; } // normalize new weights #if DEBUG printf("normalizing weights...\n"); #endif for (i = 0; i < pf->N; i++) { if (sumWeight) pf->particles[i].weight /= sumWeight; else pf->particles[i].weight = (double)1/(double)pf->N; cur_cum_weight += pf->particles[i].weight; pf->particles[i].cum_weight = cur_cum_weight; #if DEBUG printf("particle %d cum_weight = %g\n", n, pf->particles[i].cum_weight); #endif } return 0; } /* this function updates the particles in the particle filter. For each * particle, it picks a method (reinitialization, importance sampling, regular * condensation), then goes through sample, predict, correct accordingly.. * - It takes: * - a pointer to a particle filter * - a pointer to the new data * - It returns 0 (this so if need be we can return error codes) */ int pf_update(particle_filter *pf, void *predict_data, void * correct_data, void *importance_data){ double meth; double condens_probability; // how likely is this position given new_data double randVal; sampling_method method; double sum_icond_pos_likelihood= 0; particle *temp; void *temp_pos; int *temp_addr; int n, m, i, jump; double sumWeight = 0, cur_cum_weight = 0; for (n = 0; n < pf->N; n++) { // pick method for current particle #if DEBUG printf("picking method...\n"); #endif meth = rand()/(RAND_MAX+1.0); if (meth < pf->q) method = INIT; else if (meth < (pf->q)+(pf->r)) method = IMPORTANCE; else method = CONDENSATION; // sample step switch(method) { case INIT: case IMPORTANCE: // do nothing... break; case CONDENSATION: #if DEBUG printf("picking parent...\n"); #endif //pick a parent particle randVal = rand()/(RAND_MAX+1.0); i = (pf->N) / 2; jump = (pf->N) / 2; while(i >= 0 && i < (pf->N)-1) { if(i == 0 && pf->particles[i].cum_weight >= randVal) { break; } if(i == pf->N - 1 && pf->particles[i].cum_weight < randVal) { break; } if(pf->particles[i-1].cum_weight < randVal && pf->particles[i].cum_weight >= randVal) { break; } if (jump > 1) { jump /= 2; } if (pf->particles[i-1].cum_weight < randVal) { i += jump; } else { i -= jump; } #if DEBUG printf("i = %d\n", i); #endif } m = i; // we've found our parent, its index is in m #if DEBUG printf("picked %d, randVal was %lf, cum weights are %g and %g\n", m, randVal, pf->particles[m-1].cum_weight, pf->particles[m].cum_weight); #endif (*(pf->copy_pos))(pf->buffer[n].position, pf->particles[m].position); pf->buffer[n].weight = pf->particles[m].weight; #if DEBUG printf("this particle done\n"); #endif } //prediction step switch(method) { case INIT: (*(pf->icond_sample_func)) (importance_data, pf->buffer[n].position); pf->buffer[n].weight = 1; break; case IMPORTANCE: pf->buffer[n].weight = 1 / (*(pf->icond_sample_func)) (importance_data, pf->buffer[n].position); break; case CONDENSATION: #if DEBUG printf("condensation prediction\n"); #endif (*(pf->predict_pos))(predict_data,pf->buffer[n].position); // update the position } // correction step #if DEBUG printf("correct\n"); #endif condens_probability = (*(pf->prob_pos))(correct_data,pf->buffer[n].position); #if DEBUG printf("got probability %lf\n", condens_probability); #endif switch(method) { case INIT: pf->buffer[n].weight = condens_probability; break; case IMPORTANCE: sum_icond_pos_likelihood= 0; for (m = 0; m < pf->N; m++) { sum_icond_pos_likelihood += pf->particles[m].weight * (*(pf->icond_pos_likelihood_func))( pf->buffer[n].position, pf->particles[m].position); } pf->buffer[n].weight = pf->buffer[n].weight * condens_probability * sum_icond_pos_likelihood; break; case CONDENSATION: pf->buffer[n].weight = condens_probability * ((pf->ignore_old_weight)? 1 : pf->particles[m].weight); } sumWeight += pf->buffer[n].weight; #if DEBUG printf("particle %d gets weight %.24lf\n", n, pf->buffer[n].weight); #endif } // normalize new weights #if DEBUG printf("normalizing weights...\n"); #endif for (n = 0; n < pf->N; n++) { if (sumWeight) pf->buffer[n].weight /= sumWeight; else pf->buffer[n].weight = (double)1/(double)pf->N; cur_cum_weight += pf->buffer[n].weight; pf->buffer[n].cum_weight = cur_cum_weight; #if DEBUG printf("particle %d cum_weight = %g\n", n, pf->particles[n].cum_weight); #endif } temp = pf->particles; pf->particles = pf->buffer; pf->buffer = temp; temp_pos = pf->positions; pf->positions = pf->buffer_positions; pf->buffer_positions = temp_pos; return 0; } /* okay, here we want a pf_info function or something, to extract the * information from the particle filter. I haven't seen anything indicating * that there are methods in use to do this more complicated than a weighted * average. * how do we want to do this? We need to return a position, right? This * could be ugly. Ideally, I'd like to keep the particles abstracted inside * the pf functions, and just pass a function in to deal with position...but * a weighted average really just needs to know what a position is and have * an array of them passed in. Yuck. Maybe this is a reason to keep an array * of positions, rather than an array of particles with an associated position? * the other option is to abondon the abstraction completely... * Let's try this: */ /* This function returns a weighted average of all particles' positions. * It takes: * - a pointer to a particle filter * - blank_pos, a pointer to a blank (zeroed) position, allocated externally * - sum_pos, a pointer to a function which takes two pointers to positions and * adds the first position (that is the fields of the first position) to the * (fields of the) second */ int pf_info(particle_filter *pf, void *blank_pos){ int n; // user's passing in the position anyways, so we can assume it's zeroed. // makes for less clutter (we'd have to pass in yet another function pointer) (*(pf->zero_pos))(blank_pos); //zero the blank_pos for(n=0; nN; n++){ (*(pf->sum_pos))(blank_pos, pf->particles[n].position, pf->particles[n].weight); } return 0; } /* this is a function to free memory used by particle filter. It takes * a pointer to the particle filter and a function pointer that, given a * pointer to a particle, frees the contents of the particle's position * struct. Note that deallocation of the position variable for each particle * is done in here and not in free_position, analogous to memory allocation * for the position variable in the pf_init function. */ int pf_free(particle_filter *pf){ int i; free(pf->positions); free(pf->particles); free(pf->buffer); free(pf->buffer_positions); free(pf); return 0; } const void* pf_positions(particle_filter *pf){ return pf->positions; } /* this function, used mainly for debugging purposes, spews out the contents * of a particle filter. * - It takes: * - a pointer to a particle filter * - a pointer to a function which takes a particle, spews it, and returns * nothing */ void pf_print(particle_filter *pf){ int n; for(n=0; nN; n++){ printf("particle %d\n", n); (*(pf->print_pos))(pf->particles[n].position); printf("weight: %lf\n", pf->particles[n].weight); printf("\n"); } } /* * XXX let's hold on to this just in case we need it again... * XXX and if we share it w/someone else, move the static arrays * XXX into the pf struct void check_memory(particle_filter *pf) { int n; for (n = 0; n < pf->N; n++) { if ((pos_addr[n] != pf->particles[n].position) || (pos_buffer_addr[n] != pf->buffer[n].position)){ printf("OY, in check_memory, position has been corrupted...\n"); printf("n = %d\n", n); printf("pos_addr[n] = %x, pf->particles[n].position = %x\n", pos_addr[n], (int)pf->particles[n].position); printf("pos_buffer_addr[n] = %x, pf->buffer[n].position = %x\n", pos_buffer_addr[n], (int)pf->buffer[n].position); fflush(stdout); kill(getpid(), SIGSEGV); } } } */