def plot_some_data(self, iteration): if iteration % 5 == 0: plt.figure(0) xpall = [p.x for p in self.particles] ypall = [p.y for p in self.particles] mx, my, mconfidence = compute_mean_point(self.particles, surrounding=250) plt.plot(xpall, ypall, 'yo') plt.plot(self.robot.x, self.robot.y, 'rD') plt.plot(mx, my, 'cD') plt.xlim([-5000, 5000]) plt.ylim([-5000, 5000]) plt.axis('equal') plt.axhline(y=-4500) plt.axhline(y=4500) plt.axvline(x=3000) plt.axvline(x=-3000) orientation = [p.orientation for p in self.particles] # A Histogram over all the Orientations of the Particles plt.figure(1) plt.hist(orientation, 50, normed=1, facecolor='green', alpha=0.75) plt.show()
def plot_some_data(self, iteration): if iteration % 10 == 0: plt.figure(0) xpall = [p.x for p in self.particles] ypall = [p.y for p in self.particles] mx, my, mconfidence, mweight = compute_mean_point(self.particles, surrounding=250) plt.plot(xpall, ypall, 'yo') plt.plot(self.robot.x, self.robot.y, 'rD') plt.plot(mx, my, 'cD') for i in range(len(self.list_of_marks)): plt.plot(self.list_of_marks[i][0], self.list_of_marks[i][1], 'ms') plt.xlim([-5000, 5000]) plt.ylim([-5000, 5000]) for i in range(len(self.list_of_marks)): if i not in self.robot.features: continue a, w = self.robot.features[i] a += self.robot.orientation xl = [self.robot.x+x*math.cos(a*2*math.pi/360) for x in range(10000)] yl = [self.robot.y+y*math.sin(a*2*math.pi/360) for y in range(10000)] plt.plot(xl, yl, 'g-') orientation = [p.orientation for p in self.particles] # A Histogram over all the Orientations of the Particles plt.figure(1) bins = plt.hist(orientation, 50, normed=1, facecolor='green', alpha=0.75) print bins[1] wlst = [0 for i in range(len(bins[1]))] for p in self.particles: idx = len([1 for h in bins[1] if p.orientation < h]) -1 wlst[idx] += p.w print "Wlist", wlst plt.plot(bins[1], wlst, "rd") plt.show()
def algorithm_run(self): ROUNDS = 100 for iteration in range(ROUNDS): # Calculate what the robot is sensing with noise self.calculate_distances_for_defined_particle(self.robot, noise=100) # Calculate what the particles are sensing for p in self.particles: self.calculate_distances_for_defined_particle(p) # Calculate the Similarity between Particles and Robots Sensing and assign weight based on that self.calculate_similarity_and_assign_weight() # Normalize the Weight of the Particles self.normalize_particles(self.particles) # Analyze some data in each Step orientations = [p.orientation for p in self.particles] orientations_average = float(sum(orientations)) / len(orientations) orientation_std_deviation = sum([(orientations_average - o)**2 for o in orientations]) / len(orientations) print "Iteration Step", iteration print "Orienation Average/StdDev", orientations_average, orientation_std_deviation print "Orientation Robot", self.robot.orientation mx, my, mconfidence = compute_mean_point(self.particles, surrounding=250) print "PointCount within Surrounding Range:", mconfidence print "Robot calc position", int(mx), int(my) print "Robot real position", self.robot.x, self.robot.y self.plot_some_data(iteration) # Generate new Particles based on the Weighted Distribution self.particles = self.make_new_particle_cloud(self.particles)
def algorithm_run(self): ROUNDS = 100 for iteration in range(ROUNDS): # Calculate what the robot is sensing with noise self.calculate_distances_for_defined_particle(self.robot, noise=5) # Calculate what the particles are sensing for p in self.particles: self.calculate_distances_for_defined_particle(p) # Calculate the Similarity between Particles and Robots Sensing and assign weight based on that self.calculate_similarity_and_assign_weight() # Normalize the Weight of the Particles self.normalize_particles(self.particles) # Analyze some data in each Step orientations = [p.orientation for p in self.particles] orientations_average = float(sum(orientations)) / len(orientations) orientation_std_deviation = sum([(orientations_average - o) ** 2 for o in orientations]) / len(orientations) print "Iteration Step", iteration print "Orienation Average/StdDev", orientations_average, orientation_std_deviation print "Orientation Robot", self.robot.orientation mx, my, mconfidence, mweight = compute_mean_point(self.particles, surrounding=250) print "PointCount within Surrounding Range and weight:", mconfidence, mweight print "Robot calc position", int(mx), int(my) print "Robot real position", self.robot.x, self.robot.y print "Distance:", math.sqrt((int(mx) - self.robot.x) ** 2 + (int(my) - self.robot.y) ** 2) self.plot_some_data(iteration) mx, my, mconfidence, mweight = compute_mean_point(self.particles, surrounding=50) mweight *= PARTICLE_COUNT self.data_collector.add_data_point("NearC", mconfidence) self.data_collector.add_data_point("NearW", mweight) mx, my, mconfidence, mweight = compute_mean_point(self.particles, surrounding=150) mweight *= PARTICLE_COUNT self.data_collector.add_data_point("MidC", mconfidence) self.data_collector.add_data_point("MidW", mweight) mx, my, mconfidence, mweight = compute_mean_point(self.particles, surrounding=350) mweight *= PARTICLE_COUNT self.data_collector.add_data_point("FarC", mconfidence) self.data_collector.add_data_point("FarW", mweight) # Generate new Particles based on the Weighted Distribution self.particles = self.make_new_particle_cloud(self.particles) # Simulate the movement of the Robot and update all particles too self.simulate_robot_movement() plt.figure(2) plt.plot( range(len(self.data_collector.data_collection["NearC"])), self.data_collector.data_collection["NearC"], "r-" ) plt.plot( range(len(self.data_collector.data_collection["NearW"])), self.data_collector.data_collection["NearW"], "r-..", ) plt.plot( range(len(self.data_collector.data_collection["MidC"])), self.data_collector.data_collection["MidC"], "b-" ) plt.plot( range(len(self.data_collector.data_collection["MidW"])), self.data_collector.data_collection["MidW"], "b-.." ) plt.plot( range(len(self.data_collector.data_collection["FarC"])), self.data_collector.data_collection["FarC"], "c-" ) plt.plot( range(len(self.data_collector.data_collection["FarW"])), self.data_collector.data_collection["FarW"], "c-.." ) plt.show()