WIP: Hierarchical Tracker Components

examples/prh_example/fusion.py

@@ -0,0 +1,71 @@
+import numpy as np
+
+from stonesoup.custom.types.tracklet import SensorTracks
+
+
+# Create a class to represent a node in a fusion hierarchy,
+class FusionNode:
+    """
+    Class to represent a node in a fusion hierarchy
+    """
+    def __init__(self, tracker, children, statedim, use_two_state_tracks=True):
+        # Tracker used at this node
+        self.tracker = tracker
+        # Child nodes used to supply tracks for fusion
+        self.children = children
+        # The dimension of a (single) target state
+        self.statedim = statedim
+        # Whether to propagate two-state tracks
+        self.use_two_state_tracks = use_two_state_tracks
+
+    @property
+    def tracks(self):
+        return self.tracker.tracks
+
+    @property
+    def detections(self):
+        if self.is_leaf():
+            return self.tracker.detector.detections
+        else:
+            sensor_scans = [d for scan in self.tracker._scans for d in scan.sensor_scans]
+            detections = set([d for sscan in sensor_scans for d in sscan.detections])
+            return detections
+
+    # Process the tracks at this node by reading in the child tracks, creating pseudomeasurements and performing
+    # tracking
+    def process_tracks(self, timestamp):
+        child_tracks = [child.tracker.tracks for child in self.children]
+        input_tracks = [SensorTracks(tracks, i) for i, tracks in enumerate(child_tracks)]
+        if not self.use_two_state_tracks:
+            input_tracks = [SensorTracks(to_single_state(track, self.statedim), i) for i, track in enumerate(input_tracks)]
+        two_state_tracks = self.tracker.process_tracks(input_tracks, timestamp)
+        if not self.use_two_state_tracks:
+            return to_single_state(two_state_tracks, self.statedim)
+        else:
+            return two_state_tracks
+
+    def is_leaf(self):
+        return len(self.children) == 0
+
+    def get_leaf_trackers(self):
+        if self.is_leaf():
+            return [self.tracker]
+        else:
+            leaf_trackers = []
+            for child in self.children:
+                leaf_trackers += child.get_leaf_trackers()
+        return leaf_trackers
+
+    """
+    # TODO: Try to make the fusion engine more pythonic with some class structure (under construction)
+    def runTracker(self, timestamp):
+        if not self.is_leaf:
+            child_tracks = []
+            for child in self.child_trackers:
+                child_tracks.append(child.runTracker(timestamp))
+        else:
+            for child in self.child_trackers:
+                child_tracks.append()
+            child_
+        two_state_tracks
+    """

examples/prh_example/output_files.py

@@ -0,0 +1,58 @@
+import numpy as np
+
+
+def output_tracks(filename, start_time, all_tracks):
+    # Output all the tracks from the trackers
+    f = open(filename, "w")
+    f.write(str(start_time) + "\n") # start time
+    f.write(str(len(all_tracks)) + "\n") # number of hierarchy levels
+    for i_level, level in enumerate(all_tracks):
+        f.write(str(i_level) + "\n") # index of this level
+        f.write(str(len(level)) + "\n") # number of trackers at this level
+        for i_tracker, tracker in enumerate(level):
+            f.write(str(i_tracker) + "\n") # index of this tracker
+            f.write(str(len(tracker)) + "\n") # number of tracks
+            for i_track, track in enumerate(tracker):
+                f.write(str(len(track)) + "\n") # number of states in track
+                for x in track:
+                    f.write(str(x.timestamp) + "\n") # state timestamp
+                    f.write(str(x.state_vector.flatten()) + "\n") # state value
+                    f.write(str(x.covar.flatten()) + "\n") # covariance
+    f.close()
+
+
+def output_meas(filename, start_time, platform_positions, all_detections):
+    np.set_printoptions(linewidth=np.inf)
+    f = open(filename, "w")
+    f.write(str(start_time) + "\n")
+    f.write(str(len(platform_positions)) + "\n")
+    for pos in platform_positions:
+        f.write(str(pos.flatten()) + "\n")
+    f.write(str(len(all_detections)) + "\n")
+    for i_level, level_det in enumerate(all_detections):
+        f.write(str(i_level) + "\n")
+        f.write(str(len(level_det)) + "\n")
+        for i_node, node_det in enumerate(level_det):
+            f.write(str(i_node) + "\n")
+            f.write(str(len(node_det)) + "\n")
+            for det in node_det:
+                f.write(str(det.timestamp) + "\n")
+                f.write(str(np.array(det.state_vector).flatten()) + "\n")
+                f.write(str(np.array(det.measurement_model.noise_covar).flatten()) + "\n")
+                if i_level > 0:
+                    f.write(str(np.array(det.measurement_model.h_matrix.shape)) + "\n")
+                    f.write(str(np.array(det.measurement_model.h_matrix).flatten()) + "\n")
+    f.close()
+
+
+def output_truth(filename, start_time, truth):
+    # Output the truth
+    f = open(filename, "w")
+    f.write(str(start_time) + "\n")
+    f.write(str(len(truth)) + "\n")
+    for target in truth:
+        f.write(str(len(target.states)) + "\n")
+        for x in target.states:
+            f.write(str(x.timestamp) + "\n")
+            f.write(str(x.state_vector.flatten()) + "\n")
+    f.close()

examples/prh_example/prh_funcs.py

@@ -0,0 +1,65 @@
+import numpy as np
+from stonesoup.types.array import StateVector, CovarianceMatrix
+
+def tile_with_circles(minpos, maxpos, numx, numy):
+    """
+    Return centres and radius of a grid of circles with numx columns and numy rows which tile the region defined
+    by minpos and maxpos
+    """
+    field_size = maxpos - minpos
+    grid_size = max(field_size[0] / numx, field_size[1] / numy)
+    radius = grid_size / np.sqrt(2);
+    grid_start = (field_size - np.array([numx - 1, numy - 1]) * grid_size) / 2.0 + minpos
+    centres = []
+    for i in range(numx):
+        for j in range(numy):
+            centres.append(grid_start + np.array([i, j]) * grid_size)
+    return centres, radius
+
+
+def merge_position_and_velocity(position, velocity, statedim, position_mapping, velocity_mapping):
+    """
+    Create a state by merging a position and a velocity
+    """
+    state = StateVector(np.zeros((1, statedim)))
+    state[position_mapping, :] = StateVector(position)
+    state[velocity_mapping, :] = StateVector(velocity)
+    return state
+
+
+def merge_position_and_velocity_covariance(poscov, velcov, statedim, position_mapping, velocity_mapping):
+    """
+    Create a state covariance by merging a position covariance and a velocity covariance
+    """
+    covariance = CovarianceMatrix(np.zeros((statedim, statedim)))
+    covariance[np.ix_(position_mapping, position_mapping)] = poscov
+    covariance[np.ix_(velocity_mapping, velocity_mapping)] = velcov
+    return covariance
+
+
+def fit_normal_to_uniform(minval, maxval):
+    """
+    """
+    mean = StateVector((minval + maxval)/2.0)
+    cov = CovarianceMatrix(np.diag(np.power(maxval - minval, 2.0)/12.0))
+    return mean, cov
+
+
+def to_single_state(tracks, statedim):
+    """
+    Convert a set of tracks with two-state vectors to a set of tracks with one-state vectors
+    """
+    new_tracks = set()
+    for track in tracks:
+        states = []
+        for state in track.states:
+            if isinstance(state, Update):
+                new_state = GaussianStateUpdate(state.state_vector[-statedim:], state.covar[-statedim:, -statedim:],
+                                                hypothesis=state.hypothesis,
+                                                timestamp=state.timestamp)
+            else:
+                new_state = GaussianState(state.state_vector[-statedim:], state.covar[-statedim:, -statedim:],
+                                          timestamp=state.timestamp)
+            states.append(new_state)
+        new_tracks.add(Track(id=track.id, states=states))
+    return new_tracks