def fixed_lag_stitch_post_split(graph: MultiDiGraph,
fixed_particles: MMParticles,
new_particles: MMParticles,
new_weights: np.ndarray,
mm_model: MapMatchingModel,
max_rejections: int) -> MMParticles:
"""
Stitch together fixed_particles with samples from new_particles according to joint fixed-lag posterior
:param graph: encodes road network, simplified and projected to UTM
:param fixed_particles: trajectories before stitching time (won't be changed)
:param new_particles: trajectories after stitching time (to be resampled)
one observation time overlap with fixed_particles
:param new_weights: weights applied to new_particles
:param mm_model: MapMatchingModel
:param max_rejections: number of rejections to attempt before doing full fixed-lag stitching
0 will do full fixed-lag stitching and track ess_stitch
:return: MMParticles object
"""
n = len(fixed_particles)
full_fixed_lag_resample = max_rejections == 0
min_resample_time = new_particles.observation_times[1]
min_resample_time_indices = [
np.where(particle[:, 0] == min_resample_time)[0][0]
if particle is not None else 0 for particle in new_particles
]
originial_stitching_distances = np.array([
new_particles[j][min_resample_time_indices[j],
-1] if new_particles[j] is not None else 0
for j in range(n)
])
max_fixed_time = fixed_particles._first_non_none_particle[-1, 0]
stitch_time_interval = min_resample_time - max_fixed_time
distance_prior_evals = mm_model.distance_prior_evaluate(
originial_stitching_distances, stitch_time_interval)
fixed_last_coords = np.array([
part[0, 5:7] if part is not None else [0, 0] for part in new_particles
])
new_coords = np.array([
new_particles[j][min_resample_time_indices[j],
5:7] if new_particles[j] is not None else [0, 0]
for j in range(n)
])
deviation_prior_evals = mm_model.deviation_prior_evaluate(
fixed_last_coords, new_coords, originial_stitching_distances)
original_prior_evals = np.zeros(n)
pos_inds = new_particles.prior_norm > 1e-5
original_prior_evals[pos_inds] = distance_prior_evals[pos_inds] \
* deviation_prior_evals[pos_inds] \
* new_particles.prior_norm[pos_inds]
out_particles = fixed_particles
# Initiate some required quantities depending on whether to do rejection sampling or not
if full_fixed_lag_resample:
ess_stitch_track = np.zeros(n)
# distance_prior_bound = None
# adjusted_weights = None
else:
ess_stitch_track = None
pos_prior_bound = mm_model.pos_distance_prior_bound(
stitch_time_interval)
prior_bound = mm_model.distance_prior_bound(stitch_time_interval)
store_out_parts = fixed_particles.copy()
adjusted_weights = new_weights.copy()
adjusted_weights[original_prior_evals > 1e-5] /= original_prior_evals[
original_prior_evals > 1e-5]
adjusted_weights[original_prior_evals < 1e-5] = 0
adjusted_weights /= np.sum(adjusted_weights)
resort_to_full = False
# Iterate through particles
for j in range(n):
fixed_particle = fixed_particles[j]
# Check if particle is None
# i.e. fixed lag approx has failed
if fixed_particle is None:
out_particles[j] = None
if full_fixed_lag_resample:
ess_stitch_track[j] = 0
continue
last_edge_fixed = fixed_particle[-1]
last_edge_fixed_geom = get_geometry(graph, last_edge_fixed[1:4])
last_edge_fixed_length = last_edge_fixed_geom.length
if full_fixed_lag_resample:
# Full resampling
out_particles[j], ess_stitch_track[j] = full_fixed_lag_stitch(
fixed_particle, last_edge_fixed, last_edge_fixed_length,
new_particles, adjusted_weights, stitch_time_interval,
min_resample_time_indices, mm_model, True)
else:
# Rejection sampling
out_particles[j] = rejection_fixed_lag_stitch(
fixed_particle,
last_edge_fixed,
last_edge_fixed_length,
new_particles,
adjusted_weights,
stitch_time_interval,
min_resample_time_indices,
pos_prior_bound,
mm_model,
max_rejections,
break_on_zero=True)
if out_particles[j] is None:
# Rejection sampling reached max_rejections -> try full resampling
out_particles[j] = full_fixed_lag_stitch(
fixed_particle, last_edge_fixed, last_edge_fixed_length,
new_particles, adjusted_weights, stitch_time_interval,
min_resample_time_indices, mm_model, False)
if isinstance(out_particles[j], int) and out_particles[j] == 0:
resort_to_full = True
break
if resort_to_full:
for j in range(n):
fixed_particle = store_out_parts[j]
# Check if particle is None
# i.e. fixed lag approx has failed
if fixed_particle is None:
out_particles[j] = None
if full_fixed_lag_resample:
ess_stitch_track[j] = 0
continue
last_edge_fixed = fixed_particle[-1]
last_edge_fixed_geom = get_geometry(graph, last_edge_fixed[1:4])
last_edge_fixed_length = last_edge_fixed_geom.length
# Rejection sampling with full bound
out_particles[j] = rejection_fixed_lag_stitch(
fixed_particle, last_edge_fixed, last_edge_fixed_length,
new_particles, adjusted_weights, stitch_time_interval,
min_resample_time_indices, prior_bound, mm_model,
max_rejections)
if out_particles[j] is None:
# Rejection sampling reached max_rejections -> try full resampling
out_particles[j] = full_fixed_lag_stitch(
fixed_particle, last_edge_fixed, last_edge_fixed_length,
new_particles, adjusted_weights, stitch_time_interval,
min_resample_time_indices, mm_model, False)
if full_fixed_lag_resample:
out_particles.ess_stitch = np.append(out_particles.ess_stitch,
np.atleast_2d(ess_stitch_track),
axis=0)
# Do full resampling where fixed lag approx broke
none_inds = np.array([p is None for p in out_particles])
good_inds = ~none_inds
n_good = good_inds.sum()
if n_good == 0:
raise ValueError(
"Map-matching failed: all stitching probabilities zero,"
"try increasing the lag or number of particles")
if n_good < n:
none_inds_res_indices = np.random.choice(n,
n - n_good,
p=good_inds / n_good)
for i, j in enumerate(np.where(none_inds)[0]):
out_particles[j] = out_particles[none_inds_res_indices[i]]
if full_fixed_lag_resample:
out_particles.ess_stitch[-1,
none_inds] = 1 / (new_weights**2).sum()
return out_particles
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