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 predict_move_conv(self, pdf, offset, kernel):
n = len(pdf)
kN = len(kernel)
width = int((kN - 1) / 2)
prior = np.zeros(n)
for i in range(n):
for k in range(kN):
pdfIdx = (i + width - k - offset) % n
prior[i] = kernel[k] * pdf[pdfIdx]
return normalize(prior)
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 scaled_update(hall, belief, z, prob):
scale_ = prob / (1. - prob)
likelihood = np.ones(len(hall))
print("su.likelihood:", likelihood)
likelihood[hall == z] *= scale_
print("su.likelihood:", likelihood)
print("su.belief:", belief)
newhyp = likelihood * belief
print("su.lik*bel:", newhyp)
n = normalize(newhyp)
print("su.posterior:", n)
return n
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 scaled_update (hall, belief, z, prob):
scale_ = prob/(1-prob)
belief[hall==1] *=scale_
normalize(belief)
from filterpy.gh import GHFilter
import numpy as np
from filterpy.discrete_bayes import normalize
hallway = np.array([1, 1, 0, 0, 0, 0, 0, 0, 1, 0])
# belief = np.array([1./3, 1./3, 0, 0, 0, 0, 0, 0, 1/3, 0])
def update_belief(hall, belief, z, correct_scale):
for i, val in enumerate(hall):
if val == z:
belief[i] *= correct_scale
belief = np.array([0.1] * 10)
reading = 1 # 1 is 'door'
update_belief(hallway, belief, z=reading, correct_scale=3.)
# belief = belief / sum(belief) # normalizar
normalize(
belief
) # necessário normalizar para que o somatório dos valores resulte em 1 (ou seja, 100%)
print('belief:', belief)
print('sum =', sum(belief))
def update(likelihood, belief):
return normalize( likelihood * belief)
def update(likelihood, prior): return normalize(likelihood * prior)
def scaled_update(hall, belief, z, z_prob): scale = z_prob / (1. - z_prob) likelihood = np.ones(len(hall)) likelihood[hall==z] *= scale return normalize(likelihood * belief)
def scaled_update(hall, belief, z, z_prob): scale = z_prob / (1. - z_prob) belief[hall==z] *= scale normalize(belief)