#ifndef PF_H #define PF_H /* particle filter header file, yo. */ /* // example position struct typedef struct { int x; int y; int theta; int dx; int dy; int dtheta; } example_position_data; */ typedef enum {INIT, CONDENSATION, IMPORTANCE} sampling_method; typedef struct { void *position; // will point to data such as x, y, theta, velocity, etc. double weight; // always between 0 and 1 (normalized to sum to 1) double cum_weight; // cumulative weight; used in resampling // the idea is that each particle's cum_weight is equal to // the sum of all of its preceding particles' weights, // which then allows us to perform a binary search to // find a parent //sampling_method method; // how a particle was selected in the resampling step } particle; typedef struct { int N; // number of particles, i.e. sample size particle *particles; // array of particles particle *buffer; // a buffer for new generation seeding void *positions; // shortcut hack to positions of corresponding particles void *buffer_positions; // likewise for the buffer double q; double r; int ignore_old_weight; int pos_size; // funtion pointers: void (*print_pos)(void * /* position */); void (*predict_pos)(void * /* new data */, void * /* position */); double (*prob_pos)(void * /* new data */, void * /* position */); double (*icond_sample_func)(void * /* new data */, void * /* position */); double (*icond_pos_likelihood_func)(void * /* new generation */, void * /* old generation */); void (*copy_pos)(void * /* target position */, void * /* source position */); void (*free_pos)(void * /* position */); void (*zero_pos)(void * /* position */); void (*sum_pos)(void * /* position1 */, void * /* position2 */, double /* weight */); } particle_filter; // function prototypes particle_filter *pf_init(int num_particles, void *pdmap, 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 *)); void pf_print(particle_filter *pf); int pf_update(particle_filter *pf, void *predict_data, void *correct_data, void *importance_data); /* splitting up pf_update into three steps */ int pf_resample(particle_filter *pf); int pf_predict(particle_filter *pf, void *predict_data); int pf_correct(particle_filter *pf, void *correct_data, void *importance_data); int pf_free(particle_filter *pf); int pf_info(particle_filter *pf, void *pos); const void* pf_positions(particle_filter *pf); // XXX in case of emergency, break glass //void check_memory(particle_filter *pf); #endif