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_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 initiate_particles( graph: MultiDiGraph, first_observation: np.ndarray, n_samps: int, mm_model: MapMatchingModel = ExponentialMapMatchingModel(), d_refine: float = 1, d_truncate: float = None, ess_all: bool = True, filter_store: bool = True) -> MMParticles: """ Initiate start of a trajectory by sampling points around the first observation. Note that coordinate system of inputs must be the same, typically a UTM projection (not longtitude-latitude!). :param graph: encodes road network, simplified and projected to UTM :param mm_model: MapMatchingModel :param first_observation: cartesian coordinate in UTM :param n_samps: number of samples to generate :param d_refine: metres, resolution of distance discretisation :param d_truncate: metres, distance beyond which to assume zero likelihood probability defaults to 5 * mm_model.gps_sd :param ess_all: if true initiate effective sample size for each particle for each observation otherwise initiate effective sample size only for each observation :param filter_store: whether to initiate storage of filter particles and weights :return: MMParticles object """ gps_sd = mm_model.gps_sd if d_truncate is None: d_truncate = gps_sd * 5 start = tm() # Discretize edges within truncation dis_points, dists_to_first_obs = edges.get_truncated_discrete_edges( graph, first_observation, d_refine, d_truncate, True) if dis_points.size == 0: raise ValueError( "No edges found near initial observation: try increasing the initial_truncation" ) # Likelihood weights weights = np.exp(-0.5 / gps_sd**2 * dists_to_first_obs**2) weights /= np.sum(weights) # Sample indices according to weights sampled_indices = np.random.choice(len(weights), n_samps, replace=True, p=weights) # Output out_particles = MMParticles(dis_points[sampled_indices]) # Initiate ESS if ess_all: out_particles.ess_stitch = np.ones( (1, out_particles.n)) * out_particles.n out_particles.ess_pf = np.array([out_particles.n]) if filter_store: out_particles.filter_particles = [out_particles.copy()] out_particles.filter_weights = np.ones((1, n_samps)) / n_samps end = tm() out_particles.time += end - start return out_particles
def propose_particles(proposal_func: Callable, resample_weights: Union[None, np.ndarray], graph: MultiDiGraph, particles: MMParticles, new_observation: np.ndarray, time_interval: float, mm_model: MapMatchingModel, full_smoothing: bool = True, store_norm_quants: bool = False, **kwargs) -> Tuple[MMParticles, np.ndarray, np.ndarray]: """ Samples a single particle from the (distance discretised) optimal proposal. :param proposal_func: function to proposal single particle :param resample_weights: weights for resampling, None for no resampling :param graph: encodes road network, simplified and projected to UTM :param particles: all particles at last observation time :param new_observation: cartesian coordinate in UTM :param time_interval: time between last observation and newly received observation :param mm_model: MapMatchingModel :param full_smoothing: if True returns full trajectory otherwise returns only x_t-1 to x_t :param store_norm_quants: whether to additionally return quantities needed for gradient EM step assuming deviation prior is used :return: particle, unnormalised weight, prior_norm(_quants) """ n_samps = particles.n out_particles = particles.copy() if resample_weights is not None: resample_inds = np.random.choice(n_samps, n_samps, replace=True, p=resample_weights) not_prop_inds = np.arange( n_samps)[~np.isin(np.arange(n_samps), resample_inds)] else: resample_inds = np.arange(n_samps) not_prop_inds = [] weights = np.zeros(n_samps) prior_norms = np.zeros((n_samps, len(mm_model.distance_params) + 2)) if store_norm_quants else np.zeros(n_samps) for j in range(n_samps): in_particle = particles[resample_inds[j]] in_particle = in_particle.copy() if in_particle is not None else None out_particles[j], weights[j], prior_norms[ resample_inds[j]] = proposal_func( graph, in_particle, new_observation, time_interval, mm_model, full_smoothing=full_smoothing, store_norm_quants=store_norm_quants, **kwargs) for k in not_prop_inds: if particles[k] is not None: prior_norms[k] = proposal_func(graph, particles[k], None, time_interval, mm_model, full_smoothing=False, store_norm_quants=store_norm_quants, only_norm_const=True, **kwargs) else: prior_norms[k] = np.zeros(len(mm_model.distance_params) + 2) if store_norm_quants else 0 return out_particles, weights, prior_norms
def update_particles_flbs(graph: MultiDiGraph, particles: MMParticles, new_observation: np.ndarray, time_interval: float, mm_model: MapMatchingModel, proposal_func: Callable, lag: int = 3, max_rejections: int = 20, ess_threshold: float = 1., **kwargs) -> MMParticles: """ Joint fixed-lag update in light of a newly received observation, uses partial backward simulation runs for stitching Propose + reweight then backward simulation + fixed-lag stitching. :param graph: encodes road network, simplified and projected to UTM :param particles: unweighted particle approximation up to the previous observation time :param new_observation: cartesian coordinate in UTM :param time_interval: time between last observation and newly received observation :param mm_model: MapMatchingModel :param proposal_func: function to propagate and weight particles :param lag: fixed lag for resampling/stitching :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 :param ess_threshold: in [0,1], particle filter resamples if ess < ess_threshold * n_samps :param kwargs: any additional arguments to be passed to proposal i.e. d_refine or d_max for optimal proposal :return: MMParticles object """ start = tm() filter_particles = particles.filter_particles # Extract basic quantities n = particles.n observation_times = np.append( particles.observation_times, particles.observation_times[-1] + time_interval) m = len(observation_times) - 1 stitching_required = m > lag # Initiate particle output out_particles = particles.copy() # Which particles to propose from (out_particles have been resampled, filter_particles haven't) previous_resample = particles.ess_pf[-1] < ess_threshold * n base_particles = out_particles if previous_resample else particles.filter_particles[ -1].copy() latest_filter_particles, weights, temp_prior_norm = propose_particles( proposal_func, None, graph, base_particles, new_observation, time_interval, mm_model, full_smoothing=False, store_norm_quants=False, **kwargs) filter_particles[-1].prior_norm = temp_prior_norm # Update weights if not resampled if not previous_resample: weights *= particles.filter_weights[-1] # Normalise weights weights /= sum(weights) # Append new filter particles and weights, discard old ones start_point = 1 if stitching_required else 0 filter_particles = particles.filter_particles[start_point:] + [ latest_filter_particles ] out_particles.filter_weights = np.append( out_particles.filter_weights[start_point:], weights[np.newaxis], axis=0) # Store ESS out_particles.ess_pf = np.append(out_particles.ess_pf, 1 / np.sum(weights**2)) # Update time intervals out_particles.time_intervals = np.append(out_particles.time_intervals, time_interval) # Run backward simulation backward_particles = backward_simulate( graph, filter_particles, out_particles.filter_weights, out_particles.time_intervals[-lag:] if lag != 0 else [], mm_model, max_rejections, store_ess_back=False, store_norm_quants=True) backward_particles.prior_norm = backward_particles.dev_norm_quants[0] del backward_particles.dev_norm_quants if stitching_required: # Largest time not to be resampled max_fixed_time = observation_times[m - lag - 1] # Extract fixed particles fixed_particles = out_particles.copy() for j in range(n): if out_particles[j] is None: continue max_fixed_time_index = np.where( out_particles[j][:, 0] == max_fixed_time)[0][0] fixed_particles[j] = out_particles[j][:(max_fixed_time_index + 1)] # Stitch out_particles = fixed_lag_stitch_post_split(graph, fixed_particles, backward_particles, np.ones(n) / n, mm_model, max_rejections) else: out_particles.particles = backward_particles.particles out_particles.filter_particles = filter_particles end = tm() out_particles.time += end - start return out_particles
def fixed_lag_stitching(graph: MultiDiGraph, mm_model: MapMatchingModel, particles: MMParticles, weights: np.ndarray, lag: int, max_rejections: int) -> MMParticles: """ Split particles and resample (with stitching) coordinates after a certain time - defined by the lag parameter. :param graph: encodes road network, simplified and projected to UTM :param mm_model: MapMatchingModel :param particles: MMParticles object :param weights: shape = (n,) weights at latest observation time :param lag: fixed lag for resampling/stitching None indicates full multinomial resampling :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 """ # Bool whether to store ESS stitch quantities full_fixed_lag_resample = max_rejections == 0 # Check weights are normalised weights_sum = np.sum(weights) if weights_sum != 1: weights /= weights_sum # Extract basic quantities observation_times = particles.observation_times m = len(observation_times) - 1 n = particles.n ess_pf = 1 / np.sum(weights**2) # Initiate output out_particles = particles.copy() # If not reached lag yet do standard resampling if lag is None or m <= lag: if full_fixed_lag_resample: out_particles.ess_stitch = np.append(particles.ess_stitch, np.ones((1, n)) * ess_pf, axis=0) return multinomial(out_particles, weights) # Largest time not to be resampled max_fixed_time = observation_times[m - lag - 1] # Pre-process a bit fixed_particles = out_particles.copy() new_particles = out_particles.copy() max_fixed_time_indices = [0] * n for j in range(n): if out_particles[j] is None: continue max_fixed_time_indices[j] = np.where( out_particles[j][:, 0] == max_fixed_time)[0][0] fixed_particles[j] = out_particles[j][:(max_fixed_time_indices[j] + 1)] new_particles[j] = out_particles[j][max_fixed_time_indices[j]:] new_particles.prior_norm = out_particles.prior_norm[m - lag - 1] # Stitch out_particles = fixed_lag_stitch_post_split(graph, fixed_particles, new_particles, weights, mm_model, max_rejections) return out_particles
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