gaussian.java

一个基于PlaceLab的室内和室外的智能导航系统

/*
 * Created on Aug 2, 2004
 *
 */
package org.placelab.particlefilter;

import java.io.BufferedReader;
import java.io.BufferedWriter;
import java.io.FileInputStream;
import java.io.FileWriter;
import java.io.IOException;
import java.io.InputStreamReader;
import java.io.PrintWriter;
import java.util.Random;

import org.placelab.collections.UnsupportedOperationException;

/**
 *	This is a one-dimensional gaussian.  If you want to extend it to multiple dimensions,
 *  you'll have to change the 'double's representing position to some n-dimensional position
 *  representation, although a static bivariate gaussian computation is included.
 *  
 */
public class Gaussian {
	private double mean;
	private double sigma;
	
	public Gaussian(double mean, double sigma) {
		this.mean = mean;
		this.sigma = sigma;
		if (sigma < 0) throw new UnsupportedOperationException("Error: Gaussian sigmas must be positive");
	}
	public Gaussian(Gaussian g ) {
		this(g.mean, g.sigma);
	}
	
	/* getHeightAt
	 * ***********
	 * Returns the height of the gaussian at the given position.
	 */
	public double getHeightAt(double p) {
		double distance = mean-p;
		double scalar = 1.0 / (sigma * Math.sqrt(2.0*Math.PI));
		double exponent = -1.0 * ((distance*distance) / (2.0*sigma*sigma));
		double retVal = scalar*Math.exp(exponent);	
		return retVal;
	}

	/** Compute the Gaussian distribution function value.
	 *
	 * @see #bivariate(double,double,double,double,double,double,double)
	 */
	public static double univariate(double p, double mean, double sigma) {
    	double exp = Math.pow(p - mean, 2.0) / (2 * Math.pow(sigma, 2.0));
    	return (1.0 / (sigma * Math.sqrt(2*Math.PI))) * Math.exp(-exp);
	}

	/** Compute the bivariate Gaussian distribution function value.
	 *
	 * @param rho The correlation coefficient [-1...1].
	 *
	 * @see #univariate(double,double,double)
	 */
	public static double bivariate(double x, double y, double mean_x, double mean_y, double sd_x, double sd_y, double rho) {
		double z1 = Math.pow(x - mean_x, 2.0) / Math.pow(sd_x, 2.0);
		double z2 = (2 * rho * (x - mean_x) * (y - mean_y)) / (sd_x * sd_y);
		double z3 = Math.pow(y - mean_y, 2.0) / Math.pow(sd_y, 2.0);
		double z = z1 - z2 + z3;
		double exp = -(z / (2 * (1 - Math.pow(rho, 2.0))));
		double div = 2 * Math.PI * sd_x * sd_y * Math.sqrt(1.0 - Math.pow(rho, 2.0));
		return Math.exp(exp) / div;
	}
	
	/* drawSample
	 * **********
	 * Draws a random sample from the gaussian.
	 */
	public double drawSample() {
		int numSamples = 20;
		double total = 0;
		double tmp;
		for(int i=0; i<numSamples; i++) {
			tmp = -1.0;
			if (Math.random() < .5) {
				tmp = 1.0;
			}
			total += tmp;
		}
		
		total = total / Math.sqrt(numSamples);
		total *= this.sigma;
		total += this.mean;
		
		return total;
	}
	public double drawSample(Random randomizer) {
		int numSamples = 20;
		double total = 0;
		double tmp;
		for(int i=0; i<numSamples; i++) {
			tmp = -1.0;
			if (randomizer.nextDouble() < .5) {
				tmp = 1.0;
			}
			total += tmp;
		}
		
		total = total / Math.sqrt(numSamples);
		total *= this.sigma;
		total += this.mean;
		
		return total;
	}
	
	/* smoothBy
	 * ********
	 * This modifies 'this' gaussian by convolving it with the given Gaussian. The
	 * gaussian given in the argument remains unchanged.  The mean of 'this' will remain
	 * unchanges, but its stdev will increase.
	 * 
	 * This method isn't called 'convolveWith' because technically the mean of a convolution of
	 * gaussians is the sum of the means of the two inputs.  However for particle filters, we
	 * want to use convolution to smooth an estimate in-place, so the mean remains unchanged.
	 */
	public void smoothBy(Gaussian other) {
		this.sigma = Math.sqrt((this.sigma*this.sigma) + (other.sigma*other.sigma));
	}
	public void smoothBy(double otherSigma) {
		this.sigma = Math.sqrt((this.sigma*this.sigma) + (otherSigma*otherSigma));
	}
	
	/* multiplyBy
	 * **********
	 * This modifies 'this' Gaussian by multiplying it by the given Gaussian.  The
	 * Gaussian given in the argument remains unchanged.
	 */
	public void multiplyBy(Gaussian other) {
		double oldSigma = this.sigma;
		double oldMean = this.mean;
		this.sigma = (1.0 / ((1.0/oldSigma) + (1.0/other.sigma)));
		this.mean = this.sigma * (((1.0/oldSigma)*oldMean) + ((1.0/other.sigma)*other.mean));
	}
	
	public double getMean() {
		return mean;
	}
	public double getSigma() {
		return sigma;
	}
	public void setMean(double m) {
		mean = m;
	}
	public void setSigma(double s) {
		if (s < 0.0) throw new UnsupportedOperationException("Error: Gaussian sigmas must be positive");
		sigma = s;
	}
	
	
	/* toString
	 * ********
	 * Converts this gaussian to a string; used for generating persistent files containing 
	 * the information for a particular graph.
	 */
	public String toString() {
		return new String("mean: "+mean+" sigma: "+sigma);
	}
	/* fromString
	 * **********
	 * Generates a Gaussian from a String; the string must have the format of those created by 
	 * Gaussian.toString().
	 */
	public static Gaussian fromString(String s) {
		int startIndex = s.indexOf("mean:");
		int endIndex = s.indexOf("sigma:")-1;
		double mean = Double.parseDouble(s.substring(startIndex+6, endIndex));
		startIndex = endIndex + 8;
		double sigma = Double.parseDouble(s.substring(startIndex, s.length()));
		return new Gaussian(mean, sigma);
	}
	
	public static void main(String[] args) {		
		Gaussian g1 = new Gaussian(40.0, 3);
		Gaussian g2 = new Gaussian(2, 5.555);
		String testFileName = "c:/GaussianTest.txt";
		
		// write the gaussians to file
		try {
			PrintWriter pw = new PrintWriter(new BufferedWriter(new FileWriter(testFileName)));
			pw.write(g1.toString()+"\n");
			pw.write(g2.toString()+"\n");
			pw.flush();
			pw.close();
		} catch (IOException e){
			System.out.println("Error writing gaussian to file: "+testFileName);
			return;
		}
		
		// read the file back in
		Gaussian g;
		try {
			FileInputStream is = new FileInputStream(testFileName);
			BufferedReader br = new BufferedReader( new InputStreamReader(is));
		
			String line;
			while ((line=br.readLine()) != null) {
				g = Gaussian.fromString(line);
			}
			br.close();
			is.close();
		} catch (IOException e){
			System.out.println("Error reading gaussian file: "+testFileName);
			return;
		}
	}
}