粒子滤波

#include #include #include "data_types.h" #include "model_parameters.h" #include "condensation.h" /* All of the global information is packaged into the following two structures. `global' contains information which is constant over a run of the algorithm and `data' contains all of the current state at a given iteration. */ GlobalData global; IterationData data; /* End of global variables */ /* From here on is generic Condensation regardless of the form of model or observation used. All of the model-specific routines are found in model_specific.c */ /* This is binary search using cumulative probabilities to pick a base sample. The use of this routine makes Condensation O(NlogN) where N is the number of samples. It is probably better to pick base samples deterministically, since then the algorithm is O(N) and probably marginally more efficient, but this routine is kept here for conceptual simplicity and because it maps better to the published literature. */ int pick_base_sample(void) { double choice = uniform_random() * data.largest_cumulative_prob; int low, middle, high; low = 0; high = global.nsamples; while (high>(low+1)) { middle = (high+low)/2; if (choice > data.cumul_prob_array[middle]) low = middle; else high = middle; } return low; } /* This routine computes all of the new (unweighted) sample positions. For each sample, first a base is chosen, then the new sample position is computed by sampling from the prediction density p(x_t|x_t-1 = base). predict_sample_position is obviously model-dependent and is found in model_specific.c, but it can be replaced by any process model required. */ void predict_new_bases(void) { int n, base; for (n=0; n #include #ifdef ANSI_TERM_SEQUENCES #include #endif #include "data_types.h" #include "model_parameters.h" #include "condensation.h" /* The following routines are model-specific and should be replaced to implement an arbitrary process and observation model. */ void initialise_model_specific_defaults() { /* Set up the parameters of the simulation model, process and observation. */ global.scene.process.mean = SimulatedMean; global.scene.process.scaling = SimulatedScaling; global.scene.process.sigma = SimulatedSigma; global.scene.sigma = SimulatedMeasSigma; /* Set up the parameters of the prior distribution */ global.prior.mean = PriorMean; global.prior.sigma = PriorSigma; /* Set up the parameters of the process model */ global.process.mean = ProcessMean; global.process.scaling = ProcessScaling; global.process.sigma = ProcessSigma; /* Set up the parameters of the observation model */ global.obs.sigma = ObsSigma; /* Set up the parameters of the display model */ global.disp.histogram_width = DisplayWidth; } /* Set up the initial sample-set according to the prior model. The prior is a simple Gaussian, so each of the positions is filled in by sampling that Gaussian and the weights are initialised to be uniform. gaussian_random can be found in utility.c */ void set_up_prior_conditions() { int n; for (n=0; n=0 && which_bin < HistBins) bins[which_bin] += data.sample_weights[n] / data.largest_cumulative_prob; } for (b=0; b= lineheight+1.0) outc = '*'; else if (bins[b] >= lineheight+0.5 && bins[b] < lineheight+1.0) outc = '-'; else if (bins[b] >= lineheight && bins[b] < lineheight+0.5) outc = '_'; else outc = ' '; printf("%c", outc); } printf("/n"); } true_bin = eval_bin(data.meas.true); meas_bin = eval_bin(data.meas.observed); est_bin = eval_bin(estimated_position); for (b=0; b