def motion_model_update(axis, distance): global lastPoseX global lastPoseY global prior_x global prior_y global posterior_x global posterior_y space = 0 xOffset = 0 yOffset = 0 likelihood_x = np.zeros(100, dtype=float) g_i = 0 if axis == "x": xOffset = distance space = 2 * distance for i in range(lastPoseX + distance - 1, 100): if g_i == 3: break if i < 0: continue likelihood_x[i] = kernel[g_i] g_i += 1 #print 'likelihood_x:' #print likelihood_x space = 0 likelihood_y = np.zeros(100, dtype=float) g_i = 0 if axis == "y": yOffset = distance space = 2 * distance for i in range(lastPoseY + distance - 1, 100): if g_i == 3: break if i < 0: continue likelihood_y[i] = kernel[g_i] g_i += 1 #print 'likelihood_y:' #print likelihood_y if axis == "x": prior_x = predict(posterior_x, xOffset, kernel) #posterior_x = update(likelihood_x, prior_x) if axis == "y": prior_y = predict(posterior_y, yOffset, kernel) #posterior_y = update(likelihood_y, prior_y) #print 'post x:' #print posterior_x #print 'post y:' #print posterior_y lastPoseX += xOffset lastPoseY += yOffset
def discrete_bayes_sim(prior, kernel, measurements, z_prob, hallway): posterior = np.array([.1] * 10) priors, posteriors = [], [] for i, z in enumerate(measurements): prior = predict(posterior, 1, kernel) priors.append(prior) likelihood = lh_hallway(hallway, z, z_prob) posterior = update(likelihood, prior) posteriors.append(posterior) return priors, posteriors
def test_predictions(): s = 0. for k in range(3, 22, 2): # different kernel sizes for _ in range(1000): a = randn(100) kernel = normalize(randn(k)) move = randint(1, 200) s += sum(predict(a, move, kernel) - _predict(a, move, kernel)) assert s < 1.e-8, "sum of difference = {}".format(s)
def test_predictions(): s = 0. for k in range(3, 22, 2): # different kernel sizes for _ in range(1000): a = randn(100) kernel = normalize(randn(k)) move = randint(1, 200) s += sum(predict(a, move, kernel) - _predict(a, move, kernel)) assert s < 1.e-8, "sum of difference = {}".format(s)
def run(self): print(self.belief) drawnow(self.draw_fig, show_once=False, confirm=False) time.sleep(2) self.belief = self.predict_move_conv(self.belief, offset=0, kernel=[0.1, 0.8, 0.1]) drawnow(self.draw_fig, show_once=False, confirm=False) print(self.belief) time.sleep(2) self.belief = predict(self.belief, offset=1, kernel=[.1, .8, .1]) drawnow(self.draw_fig, show_once=False, confirm=False)
def train_filter(iterations, kernel, sensor_accuracy, move_distance, do_print=True): track = np.array([0, 1, 2, 3, 4, 5, 6, 7, 8, 9,10,11,12]) prior = np.array([1.] + [0]*12) posterior = prior[:] normalize(prior) robot = Train(len(track), kernel, sensor_accuracy) for i in range(iterations): # move the robot and robot.move(distance=move_distance) # peform prediction prior = predict(posterior, move_distance, kernel)#preve o sistema # print(prior) # and update the filter m = robot.sense() likelihood = lh_hallway(track, m, sensor_accuracy) posterior = update(likelihood, prior) index = np.argmax(posterior) if do_print: print('''time {}: pos {}, sensed {}, ''' '''at position {}'''.format( i, robot.pos, m, track[robot.pos])) print(''' estimated position is {}''' ''' with confidence {:.4f}%:'''.format( index, posterior[index]*100)) # book_plots.bar_plot(prior, ylim=(0, 1)) if iterations > 0: plt.bar(x-width/2,posterior,width,label='Filtro') # plt.bar(x,normalize(likelihood),width,label='Medição') plt.bar(x+width/2,prior,width,label='Previsao') else: m = 0 plt.bar(x,prior,width,label='Previsão') if do_print: print() print('final position is', robot.pos) index = np.argmax(posterior) print('''Estimated position is {} with ''' '''confidence {:.4f}%:'''.format( index, posterior[index]*100)) return m
def discrete_bayes_sim(self, kernel, zs, z_prob_correct, sleep=0.25): N = len(self.hallway) for i, z in enumerate(zs): self.loopIdx = i self.prior = predict(self.posterior, 1, kernel) drawnow(self.draw_fig_prior, show_once=False, confirm=False) time.sleep(sleep) likelihood = self.lh_hallway(self.hallway, z, z_prob_correct) print(self.hallway) print(likelihood) self.posterior = update(likelihood, self.prior) drawnow(self.draw_fig_posterior, show_once=False, confirm=False) time.sleep(sleep)
def train_filter(iterations, kernel, sensor_accuracy, move_distance, do_print=True): track = np.array([0, 1, 2, 3, 4, 5, 6, 7, 8, 9]) prior = np.array([.9] + [0.01] * 9) posterior = prior[:] normalize(prior) robot = Train(len(track), kernel, sensor_accuracy) for i in range(iterations): # move the robot and robot.move(distance=move_distance) # peform prediction prior = predict(posterior, move_distance, kernel) # and update the filter m = robot.sense() likelihood = lh_hallway(track, m, sensor_accuracy) posterior = update(likelihood, prior) index = np.argmax(posterior) if do_print: print('''time {}: pos {}, sensed {}, ''' '''at position {}'''.format( i, robot.pos, m, track[robot.pos])) print(''' estimated position is {}''' ''' with confidence {:.4f}%:'''.format( index, posterior[index] * 100)) bar_plot(posterior) if do_print: print() print('final position is', robot.pos) index = np.argmax(posterior) print('''Estimated position is {} with ''' '''confidence {:.4f}%:'''.format( index, posterior[index] * 100))
def run(self): likelihood = self.lh_hallway(self.hallway, z=1, prob=.75) self.posterior = update(likelihood, self.prior) self.prior2 = predict(self.posterior, 1, [.1, .8, .1]) self.draw_fig()
size = pylab.rcParams['figure.figsize'] set_figsize(x, y) yield pylab.rcParams['figure.figsize'] = size # kernel = (.1, .8, .1) # z_prob = 0.75 # hallway = np.array([1, 0, 1, 0, 0] * 2) # prior = np.array([.1] * 10) with figsize(y=5.5): measurements = [1, 0, 1, 0, 0, 1, 1, 1, 0, 0] for i, m in enumerate(measurements): likelihood = lh_hallway(hallway, z=m, z_prob=.75) posterior = update(likelihood, prior) prior = predict(posterior, 1, kernel) plt.subplot(5, 2, i+1) bar_plot(posterior, ylim=(0, .4), title='step {}'.format(i+1)) plt.tight_layout() class Train(object): def __init__(self, track_len, kernel=[1.], sensor_accuracy=.9): self.track_len = track_len self.pos = 0 self.kernel = kernel self.sensor_accuracy = sensor_accuracy def move(self, distance=1): """ move in the specified direction with some small chance of error""" self.pos += distance