def leituras_laser_evidencias(robot, particulas): """ Realiza leituras simuladas do laser para o robo e as particulas Depois incorpora a evidência calculando P(H|D) para todas as particulas Lembre-se de que a formula $P(z_t | x_t) = \alpha \prod_{j}^M{e^{\frac{-(z_j - \hat{z_j})}{2\sigma^2}}}$ responde somente P(D|Hi), em que H é a hi Esta função não precisa retornar nada, mas as partículas precisa ter o seu w recalculado. Você vai precisar calcular para o robo """ leitura_robo = inspercles.nb_lidar(robot, angles) w_total = 0 for i in range(len(particulas)): soma = 0 leitura_real = inspercles.nb_lidar(particulas[i], angles) for j in range(len(angles)): soma += norm.pdf(leitura_robo[angles[j]], loc=leitura_real[angles[j]], scale=7) particulas[i].w = soma w_total += soma for i in range(len(particulas)): particulas[i].w /= w_total
def leituras_laser_evidencias(robot, particulas): """ Realiza leituras simuladas do laser para o robo e as particulas Depois incorpora a evidência calculando P(H|D) para todas as particulas Lembre-se de que a formula $P(z_t | x_t) = \alpha \prod_{j}^M{e^{\frac{-(z_j - \hat{z_j})}{2\sigma^2}}}$ responde somente P(D|Hi), em que H é a hi Esta função não precisa retornar nada, mas as partículas precisa ter o seu w recalculado. Você vai precisar calcular para o robo """ soma_probs = 0 prob_total = 0 leitura_robo = inspercles.nb_lidar(robot, angles) for patricula in particulas: leitura_particulas = inspercles.nb_lidar(patricula, angles) for leitura in leitura_robo: soma_probs += stats.norm.pdf(leitura_particulas[leitura], leitura_robo[leitura], scale=1) prob_total += soma_probs patricula.w = soma_probs soma_probs = 0 for particula in particulas: p.normalize(prob_total)
def leituras_laser_evidencias(robot, particulas): """ Realiza leituras simuladas do laser para o robo e as particulas Depois incorpora a evidência calculando P(H|D) para todas as particulas Lembre-se de que a formula $P(z_t | x_t) = \alpha \prod_{j}^M{e^{\frac{-(z_j - \hat{z_j})}{2\sigma^2}}}$ responde somente P(Hi|D), em que H é a hi Esta função não precisa retornar nada, mas as partículas precisa ter o seu w recalculado. Você vai precisar calcular para o robo """ leitura_robo = inspercles.nb_lidar(robot, angles) for p in particulas: lp = inspercles.nb_lidar(p, angles) phd = 0 for lt in lp: for lr in leitura_robo: pdhi = math.exp((-(lp[lt] - leitura_robo[lr])**2) / (2 * (math.pow(1.5, 2)))) #pdhi = norm.pdf(lp[lt],loc=leitura_robo[lr],scale=1.5) phd += pdhi p.w *= phd # descobrindo alpha lista_a = [] for p in particulas: lista_a.append(p.w) alpha = 1 / (sum(lista_a)) for p in particulas: p.w = (p.w) * alpha
def leituras_laser_evidencias(robot, particulas): """ Realiza leituras simuladas do laser para o robo e as particulas Depois incorpora a evidência calculando P(H|D) para todas as particulas Lembre-se de que a formula $P(z_t | x_t) = \alpha \prod_{j}^M{e^{\frac{-(z_j - \hat{z_j})}{2\sigma^2}}}$ responde somente P(D|Hi), em que H é a hi Esta função não precisa retornar nada, mas as partículas precisa ter o seu w recalculado. ou !(2*3&-4*&|(&,$1-). !((&,$1-) !((&,$1-|2*3&-4*&) = !(2*3&-4*&) !(8$1-&|5+6ó,*&*). !(5+6ó,*&*) Você vai precisar calcular para o robo """ leitura_robo = inspercles.nb_lidar(robot, angles) # Voce vai precisar calcular a leitura para cada particula usando inspercles.nb_lidar e depois atualizar as probabilidades for p in particulas: leitura_part = inspercles.nb_lidar(p, angles) for j in leitura_robo: real = leitura_robo[j] estimado = leitura_part[j]
def leituras_laser_evidencias_produto(robot, particulas): """ Realiza leituras simuladas do laser para o robo e as particulas Depois incorpora a evidência calculando P(H|D) para todas as particulas Lembre-se de que a formula $P(z_t | x_t) = \alpha \prod_{j}^M{e^{\frac{-(z_j - \hat{z_j})}{2\sigma^2}}}$ responde somente P(Hi|D), em que H é a hi Esta função não precisa retornar nada, mas as partículas precisa ter o seu w recalculado. Você vai precisar calcular para o robo """ isfast = False leitura_robo = inspercles.nb_lidar(robot, angles,fast=isfast) soma = mpmath.mpf(0.0) for p in particulas: leitura_part = inspercles.nb_lidar(p, angles, fast=isfast) pdh = pdh_prod_mpf(leitura_robo, leitura_part) p.w = pdh soma+=pdh for p in particulas: if soma > 0.0: p.w/=soma p.w = float(p.w) else: p.w = 1.0
def leituras_laser_evidencias(robot, particulas): """ Realiza leituras simuladas do laser para o robo e as particulas Depois incorpora a evidência calculando P(H|D) para todas as particulas Lembre-se de que a formula $P(z_t | x_t) = \alpha \prod_{j}^M{e^{\frac{-(z_j - \hat{z_j})}{2\sigma^2}}}$ responde somente P(Hi|D), em que H é a hi Esta função não precisa retornar nada, mas as partículas precisa ter o seu w recalculado. Você vai precisar calcular para o robo """ # Voce vai precisar calcular a leitura para cada particula usando inspercles.nb_lidar e depois atualizar as probabilidades leitura_robo = inspercles.nb_lidar(robot, angles) lista_dis = [] dis = 0 total = 0 for p in particulas: leitura_part = inspercles.nb_lidar(p,angles) val_som = 0 for l in leitura_part: distancia_r = leitura_robo[l] distancia_p = leitura_part[l] dis = norm.pdf(distancia_p,loc = distancia_r, scale = 10) val_som += dis p.w = val_som total += val_som for p in particulas: p.normalize(total)
def leituras_laser_evidencias(robot, particulas): """ Realiza leituras simuladas do laser para o robo e as particulas Depois incorpora a evidência calculando P(H|D) para todas as particulas Lembre-se de que a formula $P(z_t | x_t) = \alpha \prod_{j}^M{e^{\frac{-(z_j - \hat{z_j})}{2\sigma^2}}}$ responde somente P(Hi|D), em que H é a hi Esta função não precisa retornar nada, mas as partículas precisa ter o seu w recalculado. Você vai precisar calcular para o robo """ leitura_robo = inspercles.nb_lidar(robot, angles) # Voce vai precisar calcular a leitura para cada particula usando inspercles.nb_lidar e depois atualizar as probabilidades lista = [] for i in particulas: leitura_particulas = inspercles.nb_lidar(i, angles) P = 0 for h in leitura_particulas: leitura = leitura_particulas[h] prob = norm.pdf(leitura, loc=leitura_robo[h], scale=7) P += prob if P == 0.0: P = 0.00000000000000000000000000001 lista.append(P) for j in range(len(particulas)): particulas[j].w = lista[j] * (1 / sum(lista))
def leituras_laser_evidencias(robot, particulas): """ Realiza leituras simuladas do laser para o robo e as particulas Depois incorpora a evidência calculando P(H|D) para todas as particulas Lembre-se de que a formula $P(z_t | x_t) = \alpha \prod_{j}^M{e^{\frac{-(z_j - \hat{z_j})}{2\sigma^2}}}$ responde somente P(D|Hi), em que H é a hi Esta função não precisa retornar nada, mas as partículas precisa ter o seu w recalculado. Você vai precisar calcular para o robo """ # Voce vai precisar calcular a leitura para cada particula usando inspercles.nb_lidar e depois atualizar as probabilidades leitura_robo = inspercles.nb_lidar(robot, angles) prob = 0 prob_geral = 0 for particle in particulas: prob = 0 leitura_particula = inspercles.nb_lidar(particle, angles) for angle in leitura_particula: #os angles são os mesmos de leitura_robo, então não da erro prob += norm.pdf(leitura_particula[angle], loc=leitura_robo[angle], scale=15) #incerteza do robo vai de 5 a 15cm prob_geral += prob particle.w = prob alfa = 1 / prob_geral for particle in particulas: particle.w = alfa * particle.w
def leituras_laser_evidencias(robot, particulas): """ Realiza leituras simuladas do laser para o robo e as particulas Depois incorpora a evidência calculando P(H|D) para todas as particulas Lembre-se de que a formula $P(z_t | x_t) = \alpha \prod_{j}^M{e^{\frac{-(z_j - \hat{z_j})}{2\sigma^2}}}$ responde somente P(D|Hi), em que H é a hi Usar sigma = 7 cm Esta função não precisa retornar nada, mas as partículas precisa ter o seu w recalculado. Você vai precisar calcular para o robo Você vai precisar calcular a leitura para cada particula usando inspercles.nb_lidar e depois atualizar as probabilidades """ sigma = 7 leitura_robo = inspercles.nb_lidar(robot, angles) prob = 0 soma_total = 0 for particula in particulas: p = inspercles.nb_lidar(particula, angles) soma = 0 for i in p.keys(): prob = norm.pdf(p[i], loc=leitura_robo[i], scale=sigma) soma += prob particula.w = soma soma_total += soma for particula in particulas: particula.w /= soma_total
def leituras_laser_evidencias(robot, particulas): """ Realiza leituras simuladas do laser para o robo e as particulas Depois incorpora a evidência calculando P(H|D) para todas as particulas Lembre-se de que a formula $P(z_t | x_t) = \alpha \prod_{j}^M{e^{\frac{-(z_j - \hat{z_j})}{2\sigma^2}}}$ responde somente P(Hi|D), em que H é a hi Esta função não precisa retornar nada, mas as partículas precisa ter o seu w recalculado. Você vai precisar calcular para o robo """ sigma = 10 leitura_robo = inspercles.nb_lidar(robot, angles) alpha = 0 prob_particulas = [] for p in particulas: leituras = inspercles.nb_lidar(p, angles) prob = 0 # p.w = p.w for laser in leituras.keys(): # prob+=norm.pdf(leituras[laser],loc=leitura_robo[laser],scale=sigma) prob += math.exp((-(leituras[laser] - leitura_robo[laser])**2) / (2 * math.pow(sigma, 2))) p.w *= prob alpha += prob for robot in particulas: robot.w /= alpha
def run(self): ''' Game loop. ''' while True: self.on_events(pygame.event.get()) if nonzero(self.robot_speed): self.robot.move_relative(self.robot_speed) self.leituras_robot = inspercles.nb_lidar( self.robot, projeto_pf.angles) # move particles projeto_pf.move_particulas(projeto_pf.particulas, self.robot_speed) # draw lasers # Atualiza probabilidade e posicoes projeto_pf.leituras_laser_evidencias(projeto_pf.robot, projeto_pf.particulas) # Reamostra as particulas projeto_pf.particulas = projeto_pf.reamostrar( projeto_pf.particulas) if self.on_update is not None: self.on_update(self.robot, projeto_pf.particulas, self.robot_speed) self.draw()
def __init__(self, on_update=None, background_file=None, robot=None): ''' Args: background_file(str): Background filename robot(Particle): Particle representing actual robot particles(list[Particle]): Simulation particles ''' self.on_update = on_update # Initialize view pygame.init() if background_file is None: background_file = BACKGROUND_FILE self.background = pygame.image.load(background_file) self.size = self.background.get_size() self.screen = pygame.display.set_mode(self.size) # Initialize simulation components self.robot_speed = [0, 0] # [LINEAR_x, LINEAR_y, ANGULAR] self.robot = robot if self.robot is None: self.robot = Particle(self.size[0] / 2, self.size[1] / 2) projeto_pf.particulas = projeto_pf.cria_particulas() self.leituras_robot = inspercles.nb_lidar(self.robot, projeto_pf.angles) self.ofb = pygame.Surface(self.size) self.ofb.set_colorkey((0, 0, 0))
def __init__(self, on_update=None, background_file=None, robot=None): ''' Args: background_file(str): Background filename robot(Particle): Particle representing actual robot particles(list[Particle]): Simulation particles ''' self.on_update = on_update # Initialize view pygame.init() if background_file is None: background_file = BACKGROUND_FILE self.background = pygame.image.load(background_file) self.size = self.background.get_size() self.screen = pygame.display.set_mode(self.size) # Initialize simulation components self.robot_speed = [0, 0] # [LINEAR_x, LINEAR_y, ANGULAR] self.robot = robot if self.robot is None: self.robot = Particle(self.size[0]/2, self.size[1]/2) projeto_pf.particulas = projeto_pf.cria_particulas() self.leituras_robot = inspercles.nb_lidar(self.robot, projeto_pf.angles) self.ofb = pygame.Surface(self.size) self.ofb.set_colorkey((0,0,0))
def leituras_laser_evidencias(robot, particulas): prob_total = 0 leitura_robo = inspercles.nb_lidar(robot, angles) for particula in particulas: # Reset das probs para cada particula e set da leitura soma_probs = 0 leitura_particula = inspercles.nb_lidar(particula, angles) # Soma das probs for angulo in leitura_robo.keys(): soma_probs += stats.norm.pdf(leitura_particula[angulo], leitura_robo[angulo], scale=8) # Append na prob total prob_total += soma_probs particula.w = soma_probs # Inverso da prob total for particula in particulas: particula.normalize(prob_total)
def leituras_laser_evidencias(robot, particulas): """ Realiza leituras simuladas do laser para o robo e as particulas Depois incorpora a evidência calculando P(H|D) para todas as particulas Lembre-se de que a formula $P(z_t | x_t) = \alpha \prod_{j}^M{e^{\frac{-(z_j - \hat{z_j})}{2\sigma^2}}}$ responde somente P(Hi|D), em que H é a hi Esta função não precisa retornar nada, mas as partículas precisa ter o seu w recalculado. Você vai precisar calcular para o robo """ dp = 20 # Desvio padrão do laser lasers_robo = inspercles.nb_lidar(robot, angles) #leitura real lista_laser_robo = list(lasers_robo.values()) lista_somatoria = [] # Aplica a leitura do laser para cada particula for p in particulas: lasers_particula = inspercles.nb_lidar(p, angles) lista_laser_particula = list(lasers_particula.values()) somatoria = 0 for i in range(len(lista_laser_robo)): somatoria += norm.pdf(lista_laser_particula[i], lista_laser_robo[i], dp) # Somatória de Monte Carlo #somatoria += np.exp(-(lista_laser_particula[i]-lista_laser_robo[i])/(2*dp*dp)) lista_somatoria.append(somatoria) for j in range(len(lista_somatoria)): particulas[j].w = lista_somatoria[j] / sum( lista_somatoria) # Normalização e atribuição do novo w return
def leituras_laser_evidencias(robot, particulas): """ Realiza leituras simuladas do laser para o robo e as particulas Depois incorpora a evidência calculando P(H|D) para todas as particulas Lembre-se de que a formula $P(z_t | x_t) = \alpha \prod_{j}^M{e^{\frac{-(z_j - \hat{z_j})}{2\sigma^2}}}$ responde somente P(Hi|D), em que H é a hi Esta função não precisa retornar nada, mas as partículas precisa ter o seu w recalculado. Você vai precisar calcular para o robo """ leitura_robo = inspercles.nb_lidar(robot, angles)
def leituras_laser_evidencias(robot, particulas): """ Realiza leituras simuladas do laser para o robo e as particulas Depois incorpora a evidência calculando P(H|D) para todas as particulas Lembre-se de que a formula $P(z_t | x_t) = \alpha \prod_{j}^M{e^{\frac{-(z_j - \hat{z_j})}{2\sigma^2}}}$ responde somente P(D|Hi), em que H é a hi Esta função não precisa retornar nada, mas as partículas precisa ter o seu w recalculado. Você vai precisar calcular para o robo """ leitura_robo = inspercles.nb_lidar(robot, angles) for particula in particulas: medidas = inspercles.lidar(particula, angles) for angulo in medidas: soma_probs += norm.pdf(angulos[angulo], loc=leitura_robo[angulo], scale=1) particula.W = soma_probs soma_probs = 0
def leituras_laser_evidencias(robot, particulas): leitura_robo = inspercles.nb_lidar(robot, angles) somatorio = 0 for i in particulas: """ Realiza leituras simuladas do laser para o robo e as particulas Depois incorpora a evidência calculando P(H|D) para todas as particulas Lembre-se de que a formula $P(z_t | x_t) = \alpha \prod_{j}^M{e^{\frac{-(z_j - \hat{z_j})}{2\sigma^2}}}$ responde somente P(D|Hi), em que H é a hi Esta função não precisa retornar nada, mas as partículas precisa ter o seu w recalculado. Você vai precisar calcular para o robo """ # Voce vai precisar calcular a leitura para cada particula usando inspercles.nb_lidar e depois atualizar as probabilidades def reamostrar(particulas, n_particulas = num_particulas): """ Reamostra as partículas devolvendo novas particulas sorteadas de acordo com a probabilidade e deslocadas de acordo com uma variação normal O notebook como_sortear tem dicas que podem ser úteis Depois de reamostradas todas as partículas precisam novamente ser deixadas com probabilidade igual Use 1/n ou 1, não importa desde que seja a mesma """ return particulas
def run(self): ''' Game loop. ''' while True: self.on_events(pygame.event.get()) if nonzero(self.robot_speed): self.robot.move_relative(self.robot_speed) self.leituras_robot = inspercles.nb_lidar(self.robot, projeto_pf.angles) # move particles projeto_pf.move_particulas(projeto_pf.particulas, self.robot_speed) # draw lasers # Atualiza probabilidade e posicoes projeto_pf.leituras_laser_evidencias(projeto_pf.robot, projeto_pf.particulas) # Reamostra as particulas projeto_pf.particulas = projeto_pf.reamostrar(projeto_pf.particulas) if self.on_update is not None: self.on_update(self.robot, projeto_pf.particulas, self.robot_speed) self.draw()
'''Realiza leituras simuladas do laser para o robo e as particulas Depois incorpora a evidência calculando P(H|D) para todas as particulas Lembre-se de que a formula $P(z_t | x_t) = \alpha \prod_{j}^M{e^{\frac{-(z_j - \hat{z_j})}{2\sigma^2}}}$ responde somente P(Hi|D), em que H é a hi Esta função não precisa retornar nada, mas as partículas precisam ter o seu w recalculado. Você vai precisar calcular para o robo''' # Voce vai precisar calcular a leitura para cada particula usando inspercles.nb_lidar e depois atualizar as probabilidades leituras_particulas=[] for p in particulas: leituras_particulas.append(inspercles.nb_lidar(p, angles).values()) leitura_robo = inspercles.nb_lidar(robot, angles).values() #mudar p ler das keys w=1 sigma = 1 m= len(angles) ws=[] for p in particulas: pdh=1 for j in range(m): pdh*=(math.e**((-leitura_robo[j]+leituras_particulas[j])/(2*sigma**2)) p.w *= pdh
i = 0 for p in interpoints: dst = (int(p[0]), int(p[1])) linha_baixo = (dst[0]+10, dst[1]-20) cv2.line(canvas, origin, dst, (deltac,deltac,deltac),3, lineType=cv2.LINE_AA) ra = real_angle(origin, dst) cv2.putText(canvas, "%d a %4.2f ra %4.2f d %4.2f"%(i,angles[i], ra, dists[i]), dst,font, 1, (255,0,0),2, lineType=cv2.LINE_AA) cv2.putText(canvas, "dif %4.2f"%(ra - angles[i]), linha_baixo,font, 1, (0,255,0),2, lineType=cv2.LINE_AA) deltac += 25 i+=1 plt.imshow(canvas) cv2.imwrite( "teste_array.png", canvas) leituras = inspercles.nb_lidar(particle, projeto_pf.angles) # WARNING: o nb_lidar antigo soma o ângulo da pose do robô nos ângulos relativos dos sensores # Foi renomeado para nb_lidar_old # O nblidar2 está ok!!! PAssou a ser o nb_lidar oficial deltac = 0 for a in sorted(leituras.keys()): l = leituras[a] dx = l*math.cos(a + particle.theta) dy = l*math.sin(a + particle.theta) endpoint = (int(particle.x + dx), int(particle.y + dy)) cv2.line(canvas_angle, origin, endpoint, (deltac,deltac,deltac),3, lineType=cv2.LINE_AA) cv2.putText(canvas_angle, "%4.2f %4.2f"%(a,l), endpoint,font, 1, (0,0,255),2, lineType=cv2.LINE_AA)