def Q8(): #load data, need odometry.dat for command, measurement.dat for comparision, landmark_gt.dat for measurement model # odom_data = load_data('ds1_Odometry.dat', 3, 0, [0, 4, 5]) odom_data = [[0.5, 0, 1], [0, -1 / (2 * np.pi), 1], [0.5, 0, 1], [0, 1 / (2 * np.pi), 1], [0.5, 0, 1]] landmark_gt_data = load_data('ds1_Landmark_Groundtruth.dat', 3, 0, [0, 2, 4, 6, 8]) barcode_data = load_data('ds1_Barcodes.dat', 3, 0, [0, 3]) barcode_dict = generate_barcode_dict(barcode_data) gt_data = load_data('ds1_Groundtruth.dat', 3, 0, [0, 3, 5, 7]) #init particles particle_num = 50 filter = ParticleFilter(particle_num) # init_pose = [gt_data[0][1], gt_data[0][2], gt_data[0][3]] init_pose = [0, 0, 0] filter.generate_particles(init_pose) #init environment with robot and particles particle_env = ParticleEnv(init_pose, filter.particles) maxsteps = len(odom_data) k = 0 robot_trajectory = [init_pose] robot_pos_predict_trajectory = [] for i in range(maxsteps): vel = [odom_data[i][0], odom_data[i][1]] duration = odom_data[i][2] particle_env.forward(vel, duration) #record the robot's trajectory and filter' prediction of robot's pos robot_trajectory.append(particle_env.robot_pos) #get robot's measurement in a duration, if null go next duration # k, robot_measurements = particle_env.find_available_measurement(k, odom_data[i-1][0], odom_data[i][0]) # print("robot_measurements length",len(robot_measurements)) robot_measurements = [] if (len(robot_measurements) == 0): robot_pos_predict_trajectory.append(particle_env.robot_pos_predict) i += 1 else: #robot_measurements is the list of [barcode, range, bearing] #given one duration's robot_measurements, update all particles' weights filter.update(robot_measurements) #record the filter's prediction robot_pos_predict_trajectory.append(particle_env.robot_pos_predict) # if degeneracy is too high, resample filter.resample() i += 1 print("prediction", robot_pos_predict_trajectory) plot_predict_trajectory_Q8(robot_trajectory, robot_pos_predict_trajectory)
def main(): parser = argparse.ArgumentParser(description='Laser unit test') parser.add_argument('--map', type=str, default='../data/map/wean.dat', help='Ground truth occupancy map') parser.add_argument('--log', type=str, default='../data/log/robotdata1.log', help='Robot data log file') parser.add_argument('--lfield', type=str, default='./config/lfield_40.csv', help='Likelihood field') parser.add_argument('--save', type=str, default='./test/', help='Which folder to save to.') args = parser.parse_args() # Read in map. map = Map() map.readMap(args.map) # Number of data points. num_odometry = 0; num_laser = 0; # Laser scan data. z = [] # Read in log file. with open(args.log) as log: for line in log: token = line.split() if token[0] == 'O': num_odometry += 1 elif token[0] == 'L': num_laser += 1 z.append(np.array(token[1:])) # n = len(z) # z = [z[i] for i in range(0, n-1, 3)] # Convert z into full numpy array. z = np.array(z, dtype='float64') # Load likelihood field. L = LikelihoodField(0.01, 40, 8) L.loadField(args.lfield) # Run filter filter = ParticleFilter() # filter.setSensorModel(SensorModel(map, 0.7, 0.29, 0.01)) filter.setLikelihoodField(L) filter.run(map, z, args.save)
def __init__(self): self._has_move = False # Check if left or right arrow has been pressed # Class' instancies self._static_landmarks = StaticLandmarks( [randint(1, DIM_OF_WORLD[0] - 1) for i in range(NB_OF_LANDMARKS)], [randint(1, DIM_OF_WORLD[1] - 1) for i in range(NB_OF_LANDMARKS)]) self._robot = Robot(randint(1, DIM_OF_WORLD[0] - 1), randint(1, DIM_OF_WORLD[1] - 1), 0, STD_ACTUATORS) self._distance_sensor = DistanceSensor(self._robot, self._static_landmarks, STD_SENSOR) self._particle_filter = ParticleFilter(self._robot._noisy_position, NB_OF_PARTICLES) self._GI_handler = GraphicalInterfaceHandler( self._robot._position, self._static_landmarks, self._particle_filter._particles) # Make the Gi event reactive self.cid = self._GI_handler._trajectory.figure.canvas.mpl_connect( 'key_press_event', self)
def getInference(self): if not self.hasInference: rows = self.model.getBeliefRows() cols = self.model.getBeliefCols() if Const.INFERENCE == 'particleFilter': self.inference = ParticleFilter(rows, cols) elif Const.INFERENCE == 'exactInference': self.inference = ExactInference(rows, cols) else: raise Exception(Const.INFERENCE + ' not understood') self.hasInference = True return self.inference
def main(): #pos = create_cartesian(catalina.START, catalina.ORIGIN_BOUND) test_robot = RobotSim(5.0, 5.0, 0, 0.1, 0.5, 1, 3, 0) BOUNDARY_RANGE = 500 """ for i in range(test_robot.num_of_auv - 1): x = random.uniform(- BOUNDARY_RANGE, BOUNDARY_RANGE) y = random.uniform(- BOUNDARY_RANGE, BOUNDARY_RANGE) z = random.uniform(- BOUNDARY_RANGE, BOUNDARY_RANGE) theta = 0 velocity = random.uniform(0,4) curr_traj_pt_index = 0 w_1 = random.uniform(-math.pi/2, math.pi/2) test_robot.auv_dict[i + 1] = Auv(x,y,z, theta, velocity, w_1, curr_traj_pt_index, i) """ theta = 0 velocity = 1 / 2 w_1 = random.uniform(-math.pi / 2, math.pi / 2) curr_traj_pt_index = 0 test_robot.auv_dict[1] = Auv(100, 0, 10, theta, velocity, w_1, curr_traj_pt_index, 1) test_robot.auv_dict[2] = Auv(0, 150, 10, theta, velocity, w_1, curr_traj_pt_index, 2) # load shark trajectories from csv file # the second parameter specify the ids of sharks that we want to track test_robot.setup("./data/shark_tracking_data_x.csv", "./data/shark_tracking_data_y.csv", [1, 2]) shark_state_dict = test_robot.get_all_sharks_state() # create a dictionary of all the particleFilters shark_state = shark_state_dict[1] auv_state = [test_robot.auv_dict[1], test_robot.auv_dict[2]] #list of auv objects test_robot.filter_dict[0] = ParticleFilter(shark_state.x, shark_state.y, auv_state) test_robot.main_navigation_loop() # test_robot.display_auv_trajectory() # to not show that live_graph, you can pass in "False" test_robot.main_navigation_loop(True)
''' Created on May 3, 2017 @author: vinicius ''' import numpy as np import pygame import time from particleFilter import ParticleFilter from math import cos, sin, pi from funcoesProbabilidade import sample largura, altura = 400, 400 pf = ParticleFilter() pygame.init() screen = pygame.display.set_mode((largura, altura)) done = False bg = pygame.image.load("mapa.jpg") i = 0 X = [] PosicaoRobo = (20, 20, 0) for j in range(99): X.append(pf.newRandomParticle()) #X = [] X.append((20, 20, 0)) #X.append(pf.newRandomParticle()) def move(): global i global u
minimumDetectionArea = 2000 imageSize = (480, 360) history_time = 50 image_no = 0 speed_mean = initialSpeed # Creating an instance for VideoCapture object # Your video file goes here capture = cv2.VideoCapture("Video1.mp4") #creating an instance of BackgroundSubtractor object bg_sub = cv2.createBackgroundSubtractorMOG2(history=history_time, detectShadows=False) # Creating instance of ParticleFilter object pf = ParticleFilter(N, initialConditions, imageSize) #permanent loop while the frame is available while (1): #reading the current frame ret, captured_frame = capture.read() #break the loop if no input is received if captured_frame is None: break #resizing the frame received captured_frame = cv2.resize(captured_frame, (imageSize)) #applying background subtraction method to get foreground mask
x0 = np.array([0.3, 0.5]) oscillator = XYContainer() dataGen = GenerateTestData(oscillator, x0.size) pltWorker = extPlot() numSamples = 10 samplingTime = 10.0 / float(numSamples) dt = 0.001 dtf = 0.001 procs = 0.01 obs = 0.01 (x, y) = dataGen.generateSamplePointsGG(samplingTime, numSamples, dt, x0, procs, obs) print "samplePoints generated" finalTime = samplingTime * (numSamples - 1) pf = ParticleFilter(oscillator, x0.size) pf.runFilter(y, samplingTime, dtf, procs, obs) pltWorker.plotPath(x, finalTime, 0.5) pltWorker.plotMarkers(y, finalTime, mk="*", ms=10, a=0.4) pltWorker.plotPFMarkers(pf.x, pf.w, finalTime) pltWorker.save("dists.pdf") pltWorker.clear() z = pf.getAveragePath() pltWorker.plotPath(x, finalTime, 0.5) pltWorker.plotMarkers(y, finalTime) pltWorker.plotMarkers(z, finalTime, mk="x", ms=8) pltWorker.save("sir.pdf") pltWorker.clear() ac = dataGen.calculateAccuracy(x, z) print "Particle filter error:", ac
oscillator = XYContainer() dataGen = GenerateTestData(oscillator, x0.size) pltWorker = extPlot() numSamples = 10 samplingTime = 10.0 / float(numSamples) dt = 0.001 dtf = 0.001 procs = 0.01 obs = 0.01 (x, y) = dataGen.generateSamplePointsGG(samplingTime, numSamples, dt, x0, procs, obs) print 'samplePoints generated' finalTime = samplingTime * (numSamples - 1) pf = ParticleFilter(oscillator, x0.size) pf.runFilter(y, samplingTime, dtf, procs, obs) pltWorker.plotPath(x, finalTime, 0.5) pltWorker.plotMarkers(y, finalTime, mk='*', ms=10, a=0.4) pltWorker.plotPFMarkers(pf.x, pf.w, finalTime) pltWorker.save("dists.pdf") pltWorker.clear() z = pf.getAveragePath() pltWorker.plotPath(x, finalTime, 0.5) pltWorker.plotMarkers(y, finalTime) pltWorker.plotMarkers(z, finalTime, mk='x', ms=8) pltWorker.save('sir.pdf') pltWorker.clear() ac = dataGen.calculateAccuracy(x, z) print "Particle filter error:", ac
class Base: def __init__(self): self._has_move = False # Check if left or right arrow has been pressed # Class' instancies self._static_landmarks = StaticLandmarks( [randint(1, DIM_OF_WORLD[0] - 1) for i in range(NB_OF_LANDMARKS)], [randint(1, DIM_OF_WORLD[1] - 1) for i in range(NB_OF_LANDMARKS)]) self._robot = Robot(randint(1, DIM_OF_WORLD[0] - 1), randint(1, DIM_OF_WORLD[1] - 1), 0, STD_ACTUATORS) self._distance_sensor = DistanceSensor(self._robot, self._static_landmarks, STD_SENSOR) self._particle_filter = ParticleFilter(self._robot._noisy_position, NB_OF_PARTICLES) self._GI_handler = GraphicalInterfaceHandler( self._robot._position, self._static_landmarks, self._particle_filter._particles) # Make the Gi event reactive self.cid = self._GI_handler._trajectory.figure.canvas.mpl_connect( 'key_press_event', self) # Event sensitive function, called when a key is pressed def __call__(self, event): """ Event sensitve function. Called anytime a key is pressed Used to call the filter's function, update the robot's positions and to plot the new values in the GI """ self.check_key( ) # Update the robot position w/respect to the pressed key if self._has_move == True: # Update sensor info (with noise) noisy_dist_function = self._distance_sensor.compute_noisy_distance( ) # Update the particles'position upon actuator state self._particle_filter.prediction(self._robot._v, self._robot._w, STD_ACTUATORS) # Compute particles'weight self._particle_filter.update_weights(noisy_dist_function, self._static_landmarks, STD_SENSOR) # Resample particules self._particle_filter.resample_particles() # Estimate position (barycenter) estimated_positon, covariance = self._particle_filter.estimate_position( ) # Print output # self.print_out(self._robot._noisy_position,estimated_positon,covariance) # Update the graphical interface self._GI_handler.update_GI(self._robot._position, self._robot._noisy_position, self._particle_filter._particles) self._has_move = False def check_key(self): """ Check if one of the arrows key has been pressed and update the robot's position accordingly """ if keyboard.is_pressed('right'): self._robot.move(1) self._has_move = True if keyboard.is_pressed('left'): self._has_move = True self._robot.move(0) if keyboard.is_pressed('up'): self._robot.change_orientation(1) if keyboard.is_pressed('down'): self._robot.change_orientation(0) def print_out(self, real_pos, estimated_pos, covariance): print("\nrealistic robot position :") real_pos.print_pos() print("estimated position :") estimated_pos.print_pos() print("with covariance = " + str(covariance)) def run(self): """ Loop until 'q' is pressed. The __call__ function is dealing with the event handling """ while not keyboard.is_pressed('q'): plt.show() plt.close()