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