class Game(object): def __init__(self): self._robot = Robot((XMAX/2+100,YMAX/2-25),0) #self._robot = particleGeneratorGenerator(0)() self._keydown = defaultdict(bool) self.reset_pf() def reset_pf(self): self._pf = ParticleFilter(particleGeneratorGenerator(5),N=500,regenProb=0.2) def draw(self,surf): #self._robot.sense(surf) #self._robot.draw(surf,(255,255,0)) for i in range(len(self._pf._particles)): p = self._pf._particles[i] p.draw(surf,color=(255,0,0)) def evolve(self): pause = True ds = 0 dh = 0 if self._keydown[pygame.K_x]: pause = False if self._keydown[pygame.K_r]: self.reset_pf() pause = False if self._keydown[pygame.K_UP]: pause = False ds = 2 elif self._keydown[pygame.K_DOWN]: pause = False ds = -2 if self._keydown[pygame.K_RIGHT]: pause = False dh = pi/32 elif self._keydown[pygame.K_LEFT]: pause = False dh = -pi/32 if not pause: action = (ds,dh) self._robot.evolve(action) ranges = self._robot.sense() def likelihood(p): ranges_prime = p.sense() ranges_diff = np.abs(ranges_prime - ranges) rv = 1.0 for i in range(len(ranges_diff)): #rv *= exp(-(ranges_diff[i]/SIGMA_RANGE)**2)*0.5 + 0.5 if (ranges_diff[i] / SIGMA_RANGE) > 2: rv *= 0.3 elif (ranges_diff[i] / SIGMA_RANGE) > 2: rv *= 0.5 elif (ranges_diff[i] / SIGMA_RANGE) > 1: rv *= 0.9 return rv if not self._keydown[pygame.K_h]: #self._pf.iterate(action,likelihood) self._pf._evolve(action) if self._keydown[pygame.K_x]: self._pf._update(likelihood) def handle_event(self,event): if event.type == pygame.KEYDOWN: self._keydown[event.key] = True elif event.type == pygame.KEYUP: self._keydown[event.key] = False