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()
