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