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