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