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'
Exemple #3
0
    '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)