/** * Calculates the probability of particle based on another particle's sense() * * @param measurement distance measurements from another particle's sense() * @return the probability of the particle being correct, between 0 and 1 */ public double measurementProb(float[] measurement) { double prob = 1.0; for (int i = 0; i < landmarks.length; i++) { float dist = (float) MathX.distance(x, y, landmarks[i].x, landmarks[i].y); prob *= MathX.Gaussian(dist, senseNoise, measurement[i]); } probability = prob; return prob; }
/** * Senses the distance of the particle to each of its landmarks * * @return a float array of distances to landmarks */ public float[] sense() { float[] ret = new float[landmarks.length]; for (int i = 0; i < landmarks.length; i++) { float dist = (float) MathX.distance(x, y, landmarks[i].x, landmarks[i].y); ret[i] = dist + (float) random.nextGaussian() * senseNoise; } return ret; }
protected double evaluate(double[] values) throws EvaluationException { if (values.length < 1) { throw new EvaluationException(ErrorEval.DIV_ZERO); } return MathX.average(values); }
protected double evaluate(double[] values) { return MathX.sumsq(values); }
protected double evaluate(double[] values) { return MathX.product(values); }
protected double evaluate(double[] values) { return values.length > 0 ? MathX.min(values) : 0; }