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