Merge pull request #193 from sglvladi/ultra_public_dev

JPDA and SystematicResampler Fixes (updated)
dstl · May 6, 2020 · c8e5537 · c8e5537
2 parents 7f6b95c + 254a91f
commit c8e5537
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 12 deletions.
diff --git 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
@@ -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,