Simple example of a particle filter

particle_filter.c

/**
 * particle_filter.c - A simple demonstration of a particle filter
 *
 * Example adapted from
 * http://web.mit.edu/16.412j/www/html/Advanced%20lectures/Slides/Hsaio_plinval_miller_ParticleFiltersPrint.pdf
 * 
 * Suppose you're trying to tell if it is rainy or sunny outside (a system
 * with two possible states), but you can't see out any windows. The only
 * information you have is observations you make as your boss periodically
 * walks by your room; he will either be carrying an umbrella, or not carrying
 * anything.
 *
 * If your boss is carrying an umbrella, you can say with 80% probabilty that
 * it is rainy, while if he isn't carrying an umbrella, you can say with 70%
 * confidence that it is sunny (people sometimes forget their umbrella!)
 * This information forms the observation model for the system.
 *
 * Now suppose you live somewhere like Seattle - if it is raining, chances are
 * (90%) it will keep raining. Meanwhile, if it is sunny, there is only a 70%
 * chance that the sun will keep on shining. This information is the
 * transition model for the system.
 *
 * The example begins with a uniform distribution of the system, then loops
 * infinitely. At each iteration, it will transition it's current model of the
 * system using the transition function, then ask you for an observation.
 * Based on the observation, it will apply particle filtering to give a best
 * estimate of the current state of the system.
 *
 */


#include <stdlib.h>
#include <stdio.h>
#include <time.h>
#include <math.h>


// Observation Model - given X, probability of Y
#define P_UMBRELLA_RAINY 	0.8f
#define P_UMBRELLA_SUNNY	0.2f
#define P_NUMBRELLA_RAINY	0.3f
#define P_NUMBRELLA_SUNNY	0.7f

// Transition Model - probability it will be X given it was Y last time
#define P_RAINY_RAINY		0.9f
#define P_RAINY_SUNNY		0.1f
#define P_SUNNY_SUNNY		0.7f
#define P_SUNNY_RAINY		0.3f


/**
 * Gets a random float in the range [0, 1)
 */
float rand_f() {
	return (float) rand() / RAND_MAX;
}


/**
 * Gets the last non-newline character from the given stream
 */
char get_single_char(FILE *stream) {
	char in, c;
	while(1) {
		c = fgetc(stream);
		if(c != '\n') {
			in = c;
		} else {
			break;
		}
	}

	return in;
}


/**
 * A simple particle filter demonstration
 */
int main(int argc, char **argv) {

	// Seed random
	srand(time(NULL));

	// Counter variables
	int i=0;

	printf("Initialising with uniform distribution\n");
	printf("######################################\n");

	// Start with a uniform distribution
	int num_samples = 100;
	int num_rainy_samples = num_samples / 2;
	int num_sunny_samples = num_samples / 2;

	printf("Distribution: %2.2f%% rainy, %2.2f%% sunny\n\n",
		(float)(num_rainy_samples * 100.0f / num_samples),
		(float)(num_sunny_samples * 100.0f / num_samples)
	);

	while(1) {

		// Transition the distribution
		printf("Transitioning Samples\n");

		int new_rainy_samples = 0;
		int new_sunny_samples = 0;

		for(i=0; i<num_rainy_samples; i++) {
			float p = rand_f();
			if(p <= P_RAINY_RAINY) {
				new_rainy_samples++;
			} else {
				new_sunny_samples++;
			}
		}

		for(i=0; i<num_sunny_samples; i++) {
			float p = rand_f();
			if(p <= P_SUNNY_SUNNY) {
				new_sunny_samples++;
			} else {
				new_rainy_samples++;
			}
		}

		num_rainy_samples = new_rainy_samples;
		num_sunny_samples = new_sunny_samples;

		// Make observation
		char in;
		while(1) {
			printf("Is your boss carrying an (u)mbrella, or (n)othing? ");
			in = get_single_char(stdin);

			if(in != 'u' && in != 'n') {
				continue;
			}

			break;
		}

		// Update weights
		float p_rainy, p_sunny;
		if(in == 'u') {
			// Observed an umbrella
			printf("We observed an umbrella\n");
			p_rainy = P_UMBRELLA_RAINY;
			p_sunny = P_UMBRELLA_SUNNY;
		} else {
			// Observed nothing
			printf("We observed nothing\n");
			p_rainy = P_NUMBRELLA_RAINY;
			p_sunny = P_NUMBRELLA_SUNNY;
		}

		printf("Updating weights\n");

		float total_rainy_prob = num_rainy_samples * p_rainy;
		float total_sunny_prob = num_sunny_samples * p_sunny;
		float total_prob = total_rainy_prob + total_sunny_prob;

		float individual_rainy_prob = total_rainy_prob / total_prob;
		float individual_sunny_prob = total_sunny_prob / total_prob;

		// Re-sample
		printf("Re-sampling\n");

		new_rainy_samples = 0;
		new_sunny_samples = 0;

		for(int i=0; i<num_samples; i++) {
			float r = rand_f();
			if(r <= individual_rainy_prob) {
				// We just sampled a new rainy point
				new_rainy_samples++;
			} else {
				// Got a new sunny point
				new_sunny_samples++;
			}
		}

		num_rainy_samples = new_rainy_samples;
		num_sunny_samples = new_sunny_samples;

		float prob_rainy = num_rainy_samples * 100.0f / num_samples;
		float prob_sunny = 100.0f - prob_rainy;

		printf("Distribution: %2.2f%% rainy, %2.2f%% sunny\n", prob_rainy, prob_sunny);
		float tolerance = 20.0f;
		float diff = prob_rainy - prob_sunny;
		if(abs(diff) < tolerance) {
			printf("Couldn't really say if it's rainy or sunny\n");
		} else if(diff < 0) {
			printf("It's probably sunny\n");
		} else {
			printf("It's probably rainy\n");
		}

		printf("\n");
	}

	return 0;
}