public void Initialize(Robot robot) { // Initialize particles particles = new List<Particle>(configuration.NumberOfParticles); for (int i = 0; i < configuration.NumberOfParticles; i++) { switch (robot.ExpectedZone) { case Robot.Zone.Starting: particles.Add(new Particle(field, 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 Robot.Zone.Mining: particles.Add(new Particle(field, DigX + configuration.WallMargin + MathHelper.random.NextDouble() * (DigX - 2.0 * configuration.WallMargin), configuration.WallMargin + MathHelper.random.NextDouble() * (FieldHeight - 2.0 * configuration.WallMargin), MathHelper.random.NextDouble() * 360.0)); break; case Robot.Zone.Unknown: particles.Add(new Particle(field, 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; } } // Update tracker tracker = new Tracker(); tracker.FindCenter(particles); }
public AutonomyHandler(AutonomyConfiguration configuration, ref TelemetryHandler telemetryHandler) { // Assign fields this.configuration = configuration; // Create localization logic localization = new MonteCarloLocalization(configuration); // Create robot robot = new Robot(configuration); // Create stopwatch stopwatch = new Stopwatch(); // Add telemetry callback telemetryHandler.TelemetryFeedbackProcessed += telemetryHandler_TelemetryFeedbackProcessed; // Create timer timer = new Timer(configuration.Interval); timer.Elapsed += timer_Elapsed; // Create algorithms depositionAlgorithm = new Deposition(configuration); lowRiskLocalizationAlgorithm = new LowRiskLocalization(configuration); // Create static state staticOutput = new Dictionary<CommandFields, short>(); staticOutput[Comms.CommandEncoding.CommandFields.Mode] = 1; staticOutput[Comms.CommandEncoding.CommandFields.FrontCameraState] = 0; staticOutput[Comms.CommandEncoding.CommandFields.BucketCameraState] = 0; staticOutput[Comms.CommandEncoding.CommandFields.RearCameraState] = 0; }
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); }