/// <summary> /// Calculate and set the weights of the particles based on a measurement. /// </summary> /// <param name="parCon">The particles to calculate and set the weights for</param> /// <param name="diffs">List containing the difference between the measurement and a particle value.</param> /// <param name="dist">The propability distribution of the measurement.</param> private void CalculateWeights(AbstractParticleController parCon, IList <float> diffs, IDistribution dist) { for (int index = 0; index < diffs.Count; index++) { float diff = diffs[index]; float p = (float)(dist.CDF(0, diff + Cdfmargin) - dist.CDF(0, diff - Cdfmargin)); parCon.SetWeightAt(index, parCon.GetWeightAt(index) * p); } }
/// <summary> /// Determine the result of a dimension and perform the necessary post calculation actions. /// </summary> /// <param name="cont">The particles of the dimension.</param> /// <returns>The value of the result of dimension.</returns> private float ProcessResult(AbstractParticleController cont) { if (Math.Abs(cont.Weights.Sum()) < float.Epsilon) { this.noiseGenerator.GenerateNoise(0.005f, cont); cont.SetWeights(1f / cont.Count); } else if (this.Measurements.Count == 0) { cont.SetWeights(1f / cont.Count); return(this.measurementReceived ? this.GetWeightedAverage(cont) : 0); } float result = this.GetWeightedAverage(cont); return(result); }
/// <summary> /// Initializes a new instance of the <see cref="AbstractParticleFilter"/> class. /// </summary> /// <param name="resampler"><see cref="resampler"/></param> /// <param name="noiseGenerator"><see cref="noiseGenerator"/></param> /// <param name="particleControllerX"><see cref="ParticleControllerX"/></param> /// <param name="particleControllerY">The particles for the y dimension</param> /// <param name="particleControllerZ">The particles for the z dimension</param> /// <param name="resampleNoiseSize"><see cref="resampleNoiseSize"/></param> /// <param name="smoother"><see cref="smoother"/></param> /// <param name="averageCalculator"><see cref="averageCalculator"/></param> protected AbstractParticleFilter( IResampler resampler, INoiseGenerator noiseGenerator, AbstractParticleController particleControllerX, AbstractParticleController particleControllerY, AbstractParticleController particleControllerZ, float resampleNoiseSize, ISmoother smoother, Func <float[], float> averageCalculator) { this.resampler = resampler; this.noiseGenerator = noiseGenerator; this.ParticleControllerX = particleControllerX; this.ParticleControllerY = particleControllerY; this.ParticleControllerZ = particleControllerZ; this.resampleNoiseSize = resampleNoiseSize; this.smoother = smoother; this.averageCalculator = averageCalculator; this.CurrentTimeStamp = -1; this.Measurements = new List <Measurement <Vector3> >(); }
private void Resample(AbstractParticleController particleController) { this.resampler.Resample(particleController); this.noiseGenerator.GenerateNoise(this.resampleNoiseSize, particleController); }
/// <summary> /// Calculates the weighted aveage of the particles. /// </summary> /// <param name="con">The particles to calculate the weighted average of.</param> /// <returns>The weighted average value of the particles.</returns> private float GetWeightedAverage(AbstractParticleController con) { con.NormalizeWeights(); return(con.WeightedAverage()); }
/// <summary> /// Predicts the value for the current timestamp and move the particles. /// </summary> /// <param name="cont">The dimension to predict in.</param> /// <param name="ie">The prediction algorithm to use.</param> private void Predict(AbstractParticleController cont, IExtrapolate ie) { float prediction = (float)ie.PredictChange(this.PreviousTimeStamp, this.CurrentTimeStamp); cont.AddToValues(prediction); }