def monte_carlo_localization(a, z, N, P_motion_sample, P_sensor, m, S=None):
"""Monte Carlo localization algorithm from Fig 25.9"""
def ray_cast(sensor_num, kin_state, m):
return m.ray_cast(sensor_num, kin_state)
M = len(z)
W = [0] * N
S_ = [0] * N
W_ = [0] * N
v = a['v']
w = a['w']
if S is None:
S = [m.sample() for _ in range(N)]
for i in range(N):
S_[i] = P_motion_sample(S[i], v, w)
W_[i] = 1
for j in range(M):
z_ = ray_cast(j, S_[i], m)
W_[i] = W_[i] * P_sensor(z[j], z_)
S = weighted_sample_with_replacement(N, S_, W_)
return S
def weighted_replicate(seq, weights, n):
"""Return n selections from seq, with the count of each element of
seq proportional to the corresponding weight (filling in fractions
randomly).
>>> weighted_replicate('ABC', [1, 2, 1], 4)
['A', 'B', 'B', 'C']
"""
assert len(seq) == len(weights)
weights = normalize(weights)
wholes = [int(w * n) for w in weights]
fractions = [(w * n) % 1 for w in weights]
return (flatten([x] * nx for x, nx in zip(seq, wholes)) +
weighted_sample_with_replacement(n - sum(wholes), seq, fractions))
def data_bagging(dataset, m=0):
"""Sample m examples with replacement"""
n = len(dataset.examples)
return weighted_sample_with_replacement(m or n, dataset.examples,
[1] * n)
