public void Diffuse()
{
this.Position = new PointF
{
X = this.Position.X + (float)RandomProportional.NextDouble(-_variance, _variance),
Y = this.Position.Y + (float)RandomProportional.NextDouble(-_variance, _variance)
};
}
/// <summary>
/// Update the the weights assigned to the particles
/// </summary>
/// <param name="particles"></param>
/// <param name="robot"></param>
/// <param name="R"></param>
/// <param name="landmarks"></param>
//public void Update(ref List<Particle> particles, Point robot, double R)
//{
// //lastOrientation = lastPoint.Angle(robot);
// //lastSpeed = (robot - lastPoint) / lastTime;
// //lastPoint = robot;
// //var rldist = ParticleFilterHelper.Distances(robot, landmarks, 1);
// var weights = particles.Select(p =>
// {
// var x = ParticleFilterHelper.Distances(p, landmarks, 1);
// var y = x.Select((_, i) => (new Normal(_, R)).Density(rldist[i]));
// return y.Aggregate((a, b) => a * b);
// //var x = ParticleFilterHelper.Distances(p, landmarks, 1);
// //var y = x.Select((_, i) => (new Normal(_, R)).Density(rldist[i]));
// //return y.Aggregate((a, b) => a * b);
// }).ToList();
// particles = particles.Zip(Normaliser.Normalise(weights), (a, b) => { a.Weight = b; return a; }).ToList();
//}
public List <Particle> Resample(List <Particle> particles)
{
double[] cumulativeWeights = new double[particles.Count];
int cumSumIdx = 0;
double cumSum = 0;
foreach (var p in particles)
{
cumSum += p.Weight;
cumulativeWeights[cumSumIdx++] = cumSum;
}
var maxCumWeight = cumulativeWeights[particles.Count - 1];
var minCumWeight = cumulativeWeights[0];
var filteredParticles = new List <Particle>();
double initialWeight = 1d / particles.Count;
for (int i = 0; i < particles.Count; i++)
{
var randWeight = minCumWeight + RandomProportional.NextDouble(1) * (maxCumWeight - minCumWeight);
int particleIdx = 0;
while (cumulativeWeights[particleIdx] < randWeight)
{
particleIdx++;
}
var p = particles[particleIdx];
filteredParticles.Add(p);
}
foreach (var dP in filteredParticles)
{
dP.Weight = 1d / particles.Count;
}
return(filteredParticles);
}
private IEnumerable <FeatureParticle> _filterParticles(int sampleCount)
{
double[] cumulativeWeights = new double[_particles.Count];
int cumSumIdx = 0;
double cumSum = 0;
foreach (var p in _particles)
{
cumSum += p.Weight;
cumulativeWeights[cumSumIdx++] = cumSum;
}
var maxCumWeight = cumulativeWeights[_particles.Count - 1];
var minCumWeight = cumulativeWeights[0];
var filteredParticles = new List <FeatureParticle>();
double initialWeight = 1d / _particles.Count;
for (int i = 0; i < sampleCount; i++)
{
var randWeight = minCumWeight + RandomProportional.NextDouble(1) * (maxCumWeight - minCumWeight);
int particleIdx = 0;
while (cumulativeWeights[particleIdx] < randWeight)
{
particleIdx++;
}
var p = _particles[particleIdx];
filteredParticles.Add(p);
}
return(filteredParticles);
}
public double Generate()
{
return(RandomProportional.NextDouble(_maxValue));
}
public static void Diffuse(this Particle p, double variance)
{
p.X += RandomProportional.NextDouble(-variance, variance);
p.Y += RandomProportional.NextDouble(-variance, variance);
}