public void UpdateState(lunabotics.Comms.States.VideoState state) { FrameRate = state.FrameRate; JPEGQuality = state.Quality * 5; }
public void Update(double elapsedTime, Robot robot, lunabotics.RCU.Autonomy.AutonomyHandler.Zone zone, bool useFrontRangeFinder) { // Move particles particles.ForEach(p => { p.SetVelocities(robot.TranslationalVelocity, robot.RotationalVelocity); p.Move(elapsedTime); }); // Blast for (int i = 0; i < configuration.NumberOfParticles * configuration.BlastPercent; i++) { double magnitude = configuration.BlastRadius * MathHelper.random.NextDouble(); double angle = MathHelper.random.NextDouble() * 2.0d * Math.PI; particles[MathHelper.random.Next(configuration.NumberOfParticles)].Set( tracker.X + magnitude * Math.Cos(angle), tracker.Y + magnitude * Math.Sin(angle), MathHelper.random.NextDouble() * 360.0); } // Re-initialize for (int i = 0; i < configuration.NumberOfParticles * configuration.RandomReInitPercent; i++) { particles[MathHelper.random.Next(configuration.NumberOfParticles)].Set( configuration.WallMargin + MathHelper.random.NextDouble() * (FieldWidth - 2.0 * configuration.WallMargin), configuration.WallMargin + MathHelper.random.NextDouble() * (FieldHeight - 2.0 * configuration.WallMargin), MathHelper.random.NextDouble() * 360.0); } // Kill lost particles foreach (Particle particle in particles) { if (!field.RectangleStarting.Contains((int)particle.X, (int)particle.Y)) { // Regenerate particle switch (zone) { case AutonomyHandler.Zone.Start: particle.Set( configuration.WallMargin + MathHelper.random.NextDouble() * (ObstacleX - 2.0 * configuration.WallMargin), configuration.WallMargin + MathHelper.random.NextDouble() * (FieldHeight - 2.0 * configuration.WallMargin), MathHelper.random.NextDouble() * 360.0); break; case AutonomyHandler.Zone.Obstacle: particle.Set( ObstacleX + configuration.WallMargin + MathHelper.random.NextDouble() * ((DigX - ObstacleX) - 2.0 * configuration.WallMargin), configuration.WallMargin + MathHelper.random.NextDouble() * (FieldHeight - 2.0 * configuration.WallMargin), MathHelper.random.NextDouble() * 360.0); break; case AutonomyHandler.Zone.Mining: particle.Set( DigX + configuration.WallMargin + MathHelper.random.NextDouble() * ((FieldWidth - DigX) - 2.0 * configuration.WallMargin), configuration.WallMargin + MathHelper.random.NextDouble() * (FieldHeight - 2.0 * configuration.WallMargin), MathHelper.random.NextDouble() * 360.0); break; case AutonomyHandler.Zone.Unknown: particle.Set( configuration.WallMargin + MathHelper.random.NextDouble() * (FieldWidth - 2.0 * configuration.WallMargin), configuration.WallMargin + MathHelper.random.NextDouble() * (FieldHeight - 2.0 * configuration.WallMargin), MathHelper.random.NextDouble() * 360.0); break; default: break; } } } // Measure robot sensors double[] z = robot.ReadSensors(useFrontRangeFinder); // Initialize weight sum double weightSum = 0.0; // Update particle weights particles.ForEach(p => weightSum += p.UpdateWeight(z, useFrontRangeFinder)); // Store elite Particle elite = particles.OrderByDescending(p => p.Weight).First(); // Re-weight particles double sum = 0.0; particles.ForEach(p => { double normalizedWeight = p.Weight / weightSum; p.NormalizedWeight = sum + normalizedWeight; sum += normalizedWeight; }); // Generate new population List<Particle> newParticles = new List<Particle>(configuration.NumberOfParticles); for (int i = 0; i < configuration.NumberOfParticles; i++) { // Get random double random = MathHelper.random.NextDouble(); for (int j = 0; j < configuration.NumberOfParticles; j++) { if (random < particles[j].NormalizedWeight) { //Debug.WriteLine(random + ":" + j); newParticles.Add((Particle)particles[j].Clone()); break; } } } // Put elite back into the population newParticles[MathHelper.random.Next(configuration.NumberOfParticles)] = (Particle)elite.Clone(); // Calculate confidence double confSum = 0.0d; particles.ForEach(p => confSum += p.Weight); this.confidence = confSum / (double)configuration.NumberOfParticles; // Overwrite particles particles = newParticles; // Update tracker position tracker.FindCenter(particles); }