From fab67923af363c9e31f607cceb2aafc47020046d Mon Sep 17 00:00:00 2001 From: JPompeus <51783218+JPompeus@users.noreply.github.com> Date: Tue, 26 Nov 2019 16:42:49 +0000 Subject: [PATCH 1/3] Sort particles by weight before cumsum. Particles are sorted before calculating the cumulative sum array. Thus when particles are resampled, they are drawn from the ordered distribution, rather than randomly. --- stonesoup/resampler/particle.py | 1 + 1 file changed, 1 insertion(+) diff --git a/stonesoup/resampler/particle.py b/stonesoup/resampler/particle.py index 4006d7c2a..936b2525b 100644 --- a/stonesoup/resampler/particle.py +++ b/stonesoup/resampler/particle.py @@ -24,6 +24,7 @@ def resample(self, particles): n_particles = len(particles) weight = Probability(1/n_particles) + particles = sorted(particles, key=lambda x: x.weight, reverse=False) cdf = np.cumsum([p.weight for p in particles]) particles_listed = list(particles) # Pick random starting point From 18b8ac486c7e670986ae76a639c663a22cbf3766 Mon Sep 17 00:00:00 2001 From: JPompeus <51783218+JPompeus@users.noreply.github.com> Date: Tue, 26 Nov 2019 16:44:12 +0000 Subject: [PATCH 2/3] JPDA to handle asynchronous measurements The prediction and measurement prediction were originally set for every hypothesis in the multihypothesis as for the missed detection state. This causes spurious results at the update step if the measurements in the multihypothesis are asynchronous (e.g. grouped by a time-windowing feeder class), and the sensor platform is moving. In this case, the updated state is calculated using the sensor's translation offset at the missed detection timestamp (i.e. the latest timestamp in the window), but for a position measurement taken at the measurement timestamp (which may be earlier in the window). This fix attaches the prediction/measurement prediction calculated for the measurement to the relevant SingleHypothesis, such that the update is calculated using the correct sensor offset for the measurement timestamp. --- stonesoup/dataassociator/probability.py | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/stonesoup/dataassociator/probability.py b/stonesoup/dataassociator/probability.py index de37f651b..69063b1a1 100644 --- a/stonesoup/dataassociator/probability.py +++ b/stonesoup/dataassociator/probability.py @@ -135,19 +135,20 @@ def associate(self, tracks, detections, time): # 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) + 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) From 254a91fad4da16b38754b4f5a10771bdd7abaa2c Mon Sep 17 00:00:00 2001 From: sglvladi Date: Wed, 15 Apr 2020 18:50:49 +0100 Subject: [PATCH 3/3] Minor fixes and update --- stonesoup/dataassociator/probability.py | 6 ++---- stonesoup/resampler/particle.py | 9 +++++---- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/stonesoup/dataassociator/probability.py b/stonesoup/dataassociator/probability.py index 69063b1a1..cf2c5d101 100644 --- a/stonesoup/dataassociator/probability.py +++ b/stonesoup/dataassociator/probability.py @@ -129,8 +129,7 @@ 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 @@ -141,8 +140,7 @@ def associate(self, tracks, detections, time): pro_detect_assoc = Probability.sum( joint_hypothesis.probability for joint_hypothesis in joint_hypotheses - if joint_hypothesis. - hypotheses[track].measurement is hypothesis.measurement) + if joint_hypothesis.hypotheses[track].measurement is hypothesis.measurement) single_measurement_hypotheses.append( SingleProbabilityHypothesis( diff --git a/stonesoup/resampler/particle.py b/stonesoup/resampler/particle.py index 936b2525b..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,9 +25,9 @@ def resample(self, particles): n_particles = len(particles) weight = Probability(1/n_particles) - particles = sorted(particles, key=lambda x: x.weight, reverse=False) - 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 = [] @@ -37,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,