diff --git a/stonesoup/dataassociator/probability.py b/stonesoup/dataassociator/probability.py index de37f651b..cf2c5d101 100644 --- a/stonesoup/dataassociator/probability.py +++ b/stonesoup/dataassociator/probability.py @@ -129,25 +129,24 @@ def associate(self, tracks, detections, time): SingleProbabilityHypothesis( hypotheses[track][0].prediction, MissedDetection(timestamp=time), - measurement_prediction=hypotheses[track][0] - .measurement_prediction, + measurement_prediction=hypotheses[track][0].measurement_prediction, probability=prob_misdetect)) # record hypothesis for any given Detection being associated with # this track - for detection in detections: + for hypothesis in hypotheses[track]: + if not hypothesis: + continue pro_detect_assoc = Probability.sum( joint_hypothesis.probability for joint_hypothesis in joint_hypotheses - if joint_hypothesis. - hypotheses[track].measurement is detection) + if joint_hypothesis.hypotheses[track].measurement is hypothesis.measurement) single_measurement_hypotheses.append( SingleProbabilityHypothesis( - hypotheses[track][0].prediction, - detection, - measurement_prediction=hypotheses[track][0]. - measurement_prediction, + hypothesis.prediction, + hypothesis.measurement, + measurement_prediction=hypothesis.measurement_prediction, probability=pro_detect_assoc)) result = MultipleHypothesis(single_measurement_hypotheses, True, 1) diff --git a/stonesoup/resampler/particle.py b/stonesoup/resampler/particle.py index 4006d7c2a..e631111be 100644 --- a/stonesoup/resampler/particle.py +++ b/stonesoup/resampler/particle.py @@ -1,5 +1,6 @@ # -*- coding: utf-8 -*- import numpy as np +from operator import attrgetter from .base import Resampler from ..types.numeric import Probability @@ -24,8 +25,9 @@ def resample(self, particles): n_particles = len(particles) weight = Probability(1/n_particles) - cdf = np.cumsum([p.weight for p in particles]) - particles_listed = list(particles) + particles_sorted = sorted(particles, key=attrgetter('weight'), reverse=False) + cdf = np.cumsum([p.weight for p in particles_sorted]) + # Pick random starting point u_i = np.random.uniform(0, 1 / n_particles) new_particles = [] @@ -36,7 +38,7 @@ def resample(self, particles): u_j = u_i + (1 / n_particles) * j - particle = particles_listed[np.argmax(u_j < cdf)] + particle = particles_sorted[np.argmax(u_j < cdf)] new_particles.append( Particle(particle.state_vector, weight=weight,