motion_params = { 'std_encoder':2, 'std_alpha':0.0873, 'l':2.83, 'h':0.76, 'a':3.78, 'b':0.50} sensor_params = { 'max_range':10, 'max_bearing':pi, 'std_range':1.0, 'std_bearing':0.0349, 'pd':0.95, 'clutter_rate':20} measurement_model = RangeBearingMeasurementModel(sensor_params) print('loading mat-file...') file = tables.openFile('groundtruth.mat') landmarks = file.root.staticMap[:].transpose() trajectory = file.root.traj[:].transpose() controls = file.root.controls[:].transpose() file.close() n_runs = 50 print('generating simulation inputs...') for n in xrange(n_runs): print(n) # make the noisy control inputs n_controls = size(controls,1)
file.close() print('computing ground truth for each time step') sensor_params = { 'max_range':10, 'max_bearing':pi, 'std_range':1.0, 'std_bearing':0.0349, 'pd':0.95, 'clutter_rate':20} # n_steps = trajectory.shape[1] ; n_steps = 1135 n_landmarks = landmarks.shape[1] true_maps = [] observed = zeros(n_landmarks,dtype=bool) measurement_model = RangeBearingMeasurementModel(sensor_params) for k in xrange(n_steps): # check if we have already seen everything if all( observed ): features = landmarks else: pose = trajectory[:,k] in_range = measurement_model.check_in_range(pose,landmarks) observed = logical_or(observed,in_range) features = landmarks[:,observed] true_maps.append(features) ground_truth = {'n_steps':n_steps,'true_maps':true_maps,'true_traj':trajectory} basedir = tkFileDialog.askdirectory() if len(basedir) == 0: print 'user cancelled selection'
'std_alpha': 0.0873, 'l': 2.83, 'h': 0.76, 'a': 3.78, 'b': 0.50 } sensor_params = { 'max_range': 10, 'max_bearing': pi, 'std_range': 1.0, 'std_bearing': 0.0349, 'pd': 0.95, 'clutter_rate': 20 } measurement_model = RangeBearingMeasurementModel(sensor_params) print('loading mat-file...') file = tables.openFile('groundtruth.mat') landmarks = file.root.staticMap[:].transpose() trajectory = file.root.traj[:].transpose() controls = file.root.controls[:].transpose() file.close() n_runs = 50 print('generating simulation inputs...') for n in xrange(n_runs): print(n) # make the noisy control inputs n_controls = size(controls, 1)