def test_mmparticles_trivial(self):
init_array = np.zeros((3, 6))
init_array += np.arange(3).reshape(3, 1)
mmp = MMParticles(init_array)
weights = np.array([0, 1, 0])
mmp_resampled = resampling.multinomial(mmp, weights)
for i in range(3):
npt.assert_array_equal(mmp_resampled[i],
np.array([[0, 1, 1, 1, 1, 1, 1, 0]]))
def test_list_repeated(self):
tlist = [a for a in range(10)]
weights = np.arange(1, 11)
weights = weights / weights.sum()
repeated_resample = np.array(
[resampling.multinomial(tlist, weights) for _ in range(10000)])
empirical_weights = np.array([(repeated_resample == i).mean()
for i in tlist])
npt.assert_array_almost_equal(weights, empirical_weights, decimal=2)
def test_mmparticles_repeated(self):
init_array = np.zeros((10, 6))
init_array += np.arange(10).reshape(10, 1)
mmp = MMParticles(init_array)
weights = np.arange(1, 11)
weights = weights / weights.sum()
repeated_resample = [
resampling.multinomial(mmp, weights) for _ in range(10000)
]
repeated_resample_arr = np.array(
[p.particles for p in repeated_resample])[:, :, 0, 1]
empirical_weights = np.array([(repeated_resample_arr == i).mean()
for i in np.arange(10)])
npt.assert_array_almost_equal(weights, empirical_weights, decimal=2)
def test_list_trivial(self):
tlist = [a for a in range(10)]
weights = np.zeros(10)
weights[0] = 1
self.assertEqual(resampling.multinomial(tlist, weights),
[0 for _ in range(10)])
def test_array_trivial(self):
array = np.arange(10)
weights = np.zeros(10)
weights[0] = 1
npt.assert_array_equal(resampling.multinomial(array, weights),
np.zeros(10))
def backward_simulate(graph: MultiDiGraph,
filter_particles: MMParticles,
filter_weights: np.ndarray,
time_interval_arr: np.ndarray,
mm_model: MapMatchingModel,
max_rejections: int,
verbose: bool = False,
store_ess_back: bool = None,
store_norm_quants: bool = False) -> MMParticles:
"""
Given particle filter output, run backwards simulation to output smoothed trajectories
:param graph: encodes road network, simplified and projected to UTM
:param filter_particles: marginal outputs from particle filter
:param filter_weights: weights
:param time_interval_arr: times between observations, must be length one less than filter_particles
:param mm_model: MapMatchingModel
:param max_rejections: number of rejections to attempt before doing full fixed-lag stitching
0 will do full backward simulation and track ess_back
:param verbose: print ess_pf or ess_back
:param store_ess_back: whether to store ess_back (if possible) in MMParticles object
:param store_norm_quants: if True normalisation quantities returned in out_particles
:return: MMParticles object
"""
n_samps = filter_particles[-1].n
num_obs = len(filter_particles)
if len(time_interval_arr) + 1 != num_obs:
raise ValueError(
"time_interval_arr must be length one less than that of filter_particles"
)
full_sampling = max_rejections == 0
if store_ess_back is None:
store_ess_back = full_sampling
# Multinomial resample end particles if weighted
if np.all(filter_weights[-1] == filter_weights[-1][0]):
out_particles = filter_particles[-1].copy()
else:
out_particles = multinomial(filter_particles[-1], filter_weights[-1])
if full_sampling:
ess_back = np.zeros((num_obs, n_samps))
ess_back[0] = 1 / (filter_weights[-1]**2).sum()
else:
ess_back = None
if num_obs < 2:
return out_particles
if store_norm_quants:
norm_quants = np.zeros(
(num_obs - 1, *filter_particles[0].prior_norm.shape))
for i in range(num_obs - 2, -1, -1):
next_time = filter_particles[i + 1].latest_observation_time
if not full_sampling:
pos_prior_bound = mm_model.pos_distance_prior_bound(
time_interval_arr[i])
prior_bound = mm_model.distance_prior_bound(time_interval_arr[i])
store_out_parts = out_particles.copy()
if filter_particles[i].prior_norm.ndim == 2:
prior_norm = filter_particles[i].prior_norm[:, 0]
else:
prior_norm = filter_particles[i].prior_norm
adjusted_weights = filter_weights[i].copy()
good_inds = np.logical_and(adjusted_weights != 0, prior_norm != 0)
adjusted_weights[good_inds] /= prior_norm[good_inds]
adjusted_weights[~good_inds] = 0
adjusted_weights /= adjusted_weights.sum()
if store_norm_quants:
sampled_inds = np.zeros(n_samps, dtype=int)
resort_to_full = False
for j in range(n_samps):
fixed_particle = out_particles[j].copy()
first_edge_fixed = fixed_particle[0]
first_edge_fixed_geom = get_geometry(graph, first_edge_fixed[1:4])
first_edge_fixed_length = first_edge_fixed_geom.length
fixed_next_time_index = np.where(
fixed_particle[:, 0] == next_time)[0][0]
if full_sampling:
back_output = full_backward_sample(
fixed_particle,
first_edge_fixed,
first_edge_fixed_length,
filter_particles[i],
adjusted_weights,
time_interval_arr[i],
fixed_next_time_index,
mm_model,
return_ess_back=True,
return_sampled_index=store_norm_quants)
if store_norm_quants:
out_particles[j], ess_back[
i, j], sampled_inds[j] = back_output
else:
out_particles[j], ess_back[i, j] = back_output
else:
back_output = rejection_backward_sample(
fixed_particle,
first_edge_fixed,
first_edge_fixed_length,
filter_particles[i],
adjusted_weights,
time_interval_arr[i],
fixed_next_time_index,
pos_prior_bound,
mm_model,
max_rejections,
return_sampled_index=store_norm_quants,
break_on_zero=True)
first_back_output = back_output[
0] if store_norm_quants else back_output
if first_back_output is None:
back_output = full_backward_sample(
fixed_particle,
first_edge_fixed,
first_edge_fixed_length,
filter_particles[i],
adjusted_weights,
time_interval_arr[i],
fixed_next_time_index,
mm_model,
return_ess_back=False,
return_sampled_index=store_norm_quants)
if isinstance(first_back_output,
int) and first_back_output == 0:
resort_to_full = True
break
if store_norm_quants:
out_particles[j], sampled_inds[j] = back_output
else:
out_particles[j] = back_output
if resort_to_full:
if store_norm_quants:
sampled_inds = np.zeros(n_samps, dtype=int)
for j in range(n_samps):
fixed_particle = store_out_parts[j]
first_edge_fixed = fixed_particle[0]
first_edge_fixed_geom = get_geometry(graph,
first_edge_fixed[1:4])
first_edge_fixed_length = first_edge_fixed_geom.length
fixed_next_time_index = np.where(
fixed_particle[:, 0] == next_time)[0][0]
back_output = rejection_backward_sample(
fixed_particle,
first_edge_fixed,
first_edge_fixed_length,
filter_particles[i],
adjusted_weights,
time_interval_arr[i],
fixed_next_time_index,
prior_bound,
mm_model,
max_rejections,
return_sampled_index=store_norm_quants,
break_on_zero=False)
first_back_output = back_output[
0] if store_norm_quants else back_output
if first_back_output is None:
back_output = full_backward_sample(
fixed_particle,
first_edge_fixed,
first_edge_fixed_length,
filter_particles[i],
adjusted_weights,
time_interval_arr[i],
fixed_next_time_index,
mm_model,
return_ess_back=False,
return_sampled_index=store_norm_quants)
if store_norm_quants:
out_particles[j], sampled_inds[j] = back_output
else:
out_particles[j] = back_output
if store_norm_quants:
norm_quants[i] = filter_particles[i].prior_norm[sampled_inds]
none_inds = np.array([p is None or None in p for p in out_particles])
good_inds = ~none_inds
n_good = good_inds.sum()
if n_good < n_samps:
none_inds_res_indices = np.random.choice(n_samps,
n_samps - n_good,
p=good_inds / n_good)
for i_none, j_none in enumerate(np.where(none_inds)[0]):
out_particles[j_none] = out_particles[
none_inds_res_indices[i_none]].copy()
if store_norm_quants:
norm_quants[:, j_none] = norm_quants[:,
none_inds_res_indices[
i_none]]
if store_ess_back:
out_particles.ess_back[i, none_inds] = n_samps
if verbose:
if full_sampling:
print(
str(filter_particles[i].latest_observation_time) +
" Av Backward ESS: " + str(np.mean(ess_back[i])))
else:
print(str(filter_particles[i].latest_observation_time))
if store_ess_back:
out_particles.ess_back = ess_back
if store_norm_quants:
out_particles.dev_norm_quants = norm_quants
return out_particles