粒子滤波

#include <stdlib.h> #include <stdio.h> #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<global.nsamples; ++n) { base = pick_base_sample(); predict_sample_position(n, base); } } /* Once all the unweighted sample positions have been computed using predict_new_bases, this routine computes the weights by evaluating the observation density at each of the positions. Cumulative probabilities are also computed at the same time, to permit an efficient implementation of pick_base_sample using binary search. evaluate_observation_density is obviously model-dependent and is found in model_specific.c, but it can be replaced by any observation model required. */ void calculate_base_weights(void) { int n; double cumul_total; cumul_total = 0.0; for (n=0; n<global.nsamples; ++n) { data.sample_weights[n] = evaluate_observation_density(n); data.cumul_prob_array[n] = cumul_total; cumul_total += data.sample_weights[n]; } data.largest_cumulative_prob = cumul_total; } /* Go and output the estimate for this iteration (which is a model-dependent routine found in model_specific.c) and then swap over the arrays ready for the next iteration. */ void update_after_iterating(int iteration) { Sample *temp; display_data(iteration); temp = data.new_positions; data.new_positions = data.old_positions; data.old_positions = temp; } /* obtain_observations is model-dependent and can be found in model_specific.c */ void run_filter(void) { int i; for (i=0; i<global.niterations; ++i) { obtain_observations(); /* Go make necessary measurements */ predict_new_bases(); /* Push previous state through process model */ calculate_base_weights(); /* Apply Bayesian measurement weighting */ update_after_iterating(i); /* Tidy up, display output, etc. */ } } /* This routine fills in the data structures with default constant values. It could be enhanced by reading informatino from the command line to allow e.g. N to be altered without recompiling. */ void initialise_defaults(void) { global.nsamples = NSamples; global.niterations = NIterations; initialise_model_specific_defaults(); } /* Create all the arrays, then fill in the prior distribution for the first iteration. The prior is model-dependent, so set_up_prior_conditions can be found in model_specific.c */ int initialise(int argc, char *argv[]) { initialise_defaults(); data.new_positions = malloc(sizeof(Sample) * global.nsamples); data.old_positions = malloc(sizeof(Sample) * global.nsamples); data.sample_weights = malloc(sizeof(double) * global.nsamples); data.cumul_prob_array = malloc(sizeof(double) * global.nsamples); if (!data.new_positions || !data.old_positions || !data.sample_weights || !data.cumul_prob_array) { fprintf(stderr, "Failed to allocate memory for sample arrays/n"); return 0; } set_up_prior_conditions(); return 1; } /* Tidy up */ void shut_down(void) { free(data.new_positions); free(data.old_positions); free(data.sample_weights); free(data.cumul_prob_array); } int main(int argc, char *argv[]) { if (!initialise(argc, argv)) return 1; run_filter(); shut_down(); return 0; } #include <stdio.h> #include <math.h> #ifdef ANSI_TERM_SEQUENCES #include <unistd.h> #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<global.nsamples; ++n) { data.old_positions[n] = global.prior.mean + global.prior.sigma * gaussian_random(); /* The probabilities are not normalised. */ data.cumul_prob_array[n] = (double) n; data.sample_weights[n] = 1.0; } /* The probabilities are not normalised, so store the largest value here (for simplicity of the binary search algorithm, cumul_prob_array[0] = 0). This can then be used as a multiplicative normalisation constant for the sample_weights array, as well. */ data.largest_cumulative_prob = (double) n; /* This is the initial positions the simulated object. */ data.meas.true = 0.0; } /* The process model for a first-order auto-regressive process is: x_{t+1} - mean = (x_t - mean)*scaling + sigma*w_t where w_t is unit iid Gaussian noise. gaussian_random can be found in utility.c */ float iterate_first_order_arp(float previous, ProcessModel process) { return process.mean + ((previous - process.mean) * process.scaling) + process.sigma * gaussian_random(); } /* In a real implementation, this routine would go and actually make measurements and store them in the data.meas structure. This simulation consists of an `object' moving around obeying a first-order auto-regressive process, and being observed with its true positions corrupted by Gaussian measurement noise. Accordingly, this routine calculates the new simulated true and measured position of the object. */ void obtain_observations() { data.meas.true = iterate_first_order_arp(data.meas.true, global.scene.process); data.meas.observed = data.meas.true + global.scene.sigma * gaussian_random(); } /* This routine samples from the distribution p(x_t | x_{t-1} = old_positions[old_sample]) and stores the result in new_positions[new_sample]. This is straightforward for the simple first-order auto-regressive process model used here, but any model could be substituted. */ void predict_sample_position(int new_sample, int old_sample) { data.new_positions[new_sample] = iterate_first_order_arp(data.old_positions[old_sample], global.process); } /* This routine evaluates the observation density p(z_t|x_t = new_positions[new_sample]) The observation model in this implementation is a simple mixture of Gaussians, where each simulated object is observed as a 1d position and measurement noise is represented as Gaussian. For a visual-tracking application, this routine would go and evaluate the likelihood that the object is present in the image at the position encoded by new_positions[new_sample]. evaluate_gaussian can be found in utility.c */ double evaluate_observation_density(int new_sample) { return evaluate_gaussian(data.new_positions[new_sample] - data.meas.observed, global.obs.sigma); } /* The following two routines provide rudimentary graphical output in the form of an ASCII histogram of the 1d state density. */ static int eval_bin(float where) { return (HistBins-1)/2 + (int) (where * (HistBins-1)/2 / global.disp.histogram_width); } static void display_histogram(double estimated_position) { static double bins[HistBins]; int b, n, line, which_bin, meas_bin, est_bin, true_bin; double lineheight; char outc; for (b=0; b<HistBins; ++b) bins[b] = 0.0; for (n=0; n<global.nsamples; ++n) { which_bin = eval_bin(data.new_positions[n]); if (which_bin >=0 && which_bin < HistBins) bins[which_bin] += data.sample_weights[n] / data.largest_cumulative_prob; } for (b=0; b<HistBins; ++b) bins[b] = (bins[b] * (double) HistLines) / MaxHistHeight; for (line=0; line<HistLines; ++line) { lineheight = (double) (HistLines-1-line); for (b=0; b<HistBins; ++b) { if (line==0 && bins[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<HistBins; ++b) { if ((b == meas_bin && b == est_bin) || (b == meas_bin && b == true_bin) || (b == true_bin && b == est_bin)) outc = '*'; else if (b == true_bin) outc = '.'; else if (b == meas_bin) outc = '+'; else if (b == est_bin) outc = 'x'; else outc = ' '; printf("%c", outc); } printf("/n"); } /* This routine computes the estimated position of the object by estimating the mean of the state-distribution as a weighted mean of the sample positions, then displays a histogram of the state distribution along with a character denoting the estimated position of the object. */ void display_data(int iteration) { int n; double aggregate; aggregate = 0.0; /* Compute the unnormalised weighted mean of the sample positions. */ for (n=0; n<global.nsamples; ++n) aggregate += data.new_positions[n] * data.sample_weights[n]; aggregate /= data.largest_cumulative_prob; display_histogram(aggregate); printf("%04d: Measured pos. % 3.4lf True pos. % 3.4lf Est. position % 3.4lf/n", iteration, data.meas.observed, data.meas.true, aggregate); #ifdef ANSI_TERM_SEQUENCES /* If possible, run the cursor back up the screen so the histogram stays in the same place instead of scrolling down the display. */ printf("/033[%dA", HistLines+2); sleep(1); #endif } /* End of model-specific routines */