angular_v = gyro.angular_velocity[gyro_idx]
pf.predict_all(linear_v, angular_v, gyro.delta_t)
# car_trajactory[:,gyro_idx] = pf.particles[:2,0]
# update step
if update_now:
update_count += 1
lidar_scan = lidar.polar_to_xy(encoder_idx)
# update alpha, find max particle
# max_particle, max_correlation = pf.update(lidar_scan, myMap, tf)
max_particle = pf.particles[:, 0]
car_trajactory[:, update_count] = max_particle[:2]
# use max particle to update update map
lidar_world = tf.lidar_to_world(lidar_scan, max_particle)
x_cells, y_cells = myMap.meter_to_cell(lidar_world[:2, :])
# find cells that ray pass through
scaned_cell = []
for x_cell1, y_cell1 in zip(x_cells, y_cells):
particle_x = np.round(pf.particles[0, 0] / myMap.res).astype(int)
particle_y = np.round(pf.particles[1, 0] / myMap.res).astype(int)
x_cell, y_cell = line(particle_x, particle_y, x_cell1, y_cell1)
scaned_cell.append((x_cell, y_cell))
myMap.update_log_odds(scaned_cell)
update_now = False
pf.resampling()
# print final time
