/// <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);
}
