Merge pull request #1084 from dstl/sensor_man_perf

Sensor performance improvement

stonesoup/functions/__init__.py

@@ -1,6 +1,7 @@
 """Mathematical functions used within Stone Soup"""
 import copy
 import warnings
+from functools import lru_cache
 
 import numpy as np
 
@@ -707,10 +708,12 @@ def build_rotation_matrix(angle_vector: np.ndarray):
         :class:`numpy.ndarray` of shape (3, 3)
             The model (3D) rotation matrix.
     """
-    theta_x = -angle_vector[0, 0]  # roll
-    theta_y = angle_vector[1, 0]  # pitch#elevation
-    theta_z = -angle_vector[2, 0]  # yaw#azimuth
-    return rotx(theta_x) @ roty(theta_y) @ rotz(theta_z)
+    return _build_rotation_matrix(angle_vector[0, 0], angle_vector[1, 0], angle_vector[2, 0])
+
+
+@lru_cache()
+def _build_rotation_matrix(theta_x, theta_y, theta_z):
+    return rotx(-theta_x) @ roty(theta_y) @ rotz(-theta_z)
 
 
 def build_rotation_matrix_xyz(angle_vector: np.ndarray):
@@ -734,10 +737,12 @@ def build_rotation_matrix_xyz(angle_vector: np.ndarray):
         :class:`numpy.ndarray` of shape (3, 3)
             The model (3D) rotation matrix.
     """
-    theta_x = -angle_vector[0, 0]  # roll
-    theta_y = angle_vector[1, 0]  # pitch#elevation
-    theta_z = -angle_vector[2, 0]  # yaw#azimuth
-    return rotz(theta_z) @ roty(theta_y) @ rotx(theta_x)
+    return _build_rotation_matrix_xyz(angle_vector[0, 0], angle_vector[1, 0], angle_vector[2, 0])
+
+
+@lru_cache()
+def _build_rotation_matrix_xyz(theta_x, theta_y, theta_z):
+    return rotz(-theta_z) @ roty(theta_y) @ rotx(-theta_x)
 
 
 def dotproduct(a, b):

stonesoup/sensor/passive.py

@@ -39,7 +39,7 @@ def measurement_model(self):
             translation_offset=self.position,
             rotation_offset=self.orientation)
 
-    def is_detectable(self, state: GroundTruthState) -> bool:
+    def is_detectable(self, state: GroundTruthState, measurement_model=None) -> bool:
         return True
 
     def is_clutter_detectable(self, state: Detection) -> bool:

stonesoup/sensor/radar/radar.py

@@ -59,8 +59,10 @@ def measurement_model(self):
             translation_offset=self.position,
             rotation_offset=self.orientation)
 
-    def is_detectable(self, state: GroundTruthState) -> bool:
-        measurement_vector = self.measurement_model.function(state, noise=False)
+    def is_detectable(self, state: GroundTruthState, measurement_model=None) -> bool:
+        if measurement_model is None:
+            measurement_model = self.measurement_model
+        measurement_vector = measurement_model.function(state, noise=False)
         true_range = measurement_vector[1, 0]  # Bearing(0), Range(1)
         return true_range <= self.max_range
 
@@ -102,14 +104,23 @@ def measurement_model(self):
             translation_offset=self.position,
             rotation_offset=self.orientation)
 
-    def is_detectable(self, state: GroundTruthState) -> bool:
-        tmp_meas_model = CartesianToBearingRange(
-            ndim_state=self.ndim_state,
-            mapping=self.position_mapping,
-            noise_covar=self.noise_covar,
-            translation_offset=self.position,
-            rotation_offset=self.orientation
-        )
+    def is_detectable(self, state: GroundTruthState, measurement_model=None) -> bool:
+        if measurement_model is None:
+            tmp_meas_model = CartesianToBearingRange(
+                ndim_state=self.ndim_state,
+                mapping=self.position_mapping,
+                noise_covar=self.noise_covar,
+                translation_offset=self.position,
+                rotation_offset=self.orientation
+            )
+        else:
+            tmp_meas_model = CartesianToBearingRange(
+                ndim_state=measurement_model.ndim_state,
+                mapping=measurement_model.mapping,
+                noise_covar=measurement_model.noise_covar,
+                translation_offset=measurement_model.translation_offset,
+                rotation_offset=measurement_model.rotation_offset
+            )
         measurement_vector = tmp_meas_model.function(state, noise=False)
         true_range = measurement_vector[1, 0]  # Bearing(0), Range(1)
         return true_range <= self.max_range
@@ -180,8 +191,10 @@ def measure(self, ground_truths: Set[GroundTruthState], noise: Union[np.ndarray,
 
         return super().measure(ground_truths, noise, **kwargs)
 
-    def is_detectable(self, state: GroundTruthState) -> bool:
-        measurement_vector = self.measurement_model.function(state, noise=False)
+    def is_detectable(self, state: GroundTruthState, measurement_model=None) -> bool:
+        if measurement_model is None:
+            measurement_model = self.measurement_model
+        measurement_vector = measurement_model.function(state, noise=False)
 
         # Check if state falls within sensor's FOV
         fov_min = -self.fov_angle / 2
@@ -265,21 +278,30 @@ def measure(self, ground_truths: Set[GroundTruthState], noise: Union[np.ndarray,
 
         return super().measure(ground_truths, noise, **kwargs)
 
-    def is_detectable(self, state: GroundTruthState) -> bool:
-        antenna_heading = self.orientation[2, 0] + self.dwell_centre[0, 0]
-        # Set rotation offset of underlying measurement model
-        rot_offset = \
-            StateVector(
-                [[self.orientation[0, 0]],
-                 [self.orientation[1, 0]],
-                 [antenna_heading]])
-        tmp_meas_model = CartesianToBearingRange(
-            ndim_state=self.position_mapping,
-            mapping=self.position_mapping,
-            noise_covar=self.noise_covar,
-            translation_offset=self.position,
-            rotation_offset=rot_offset
-        )
+    def is_detectable(self, state: GroundTruthState, measurement_model=None) -> bool:
+        if measurement_model is None:
+            antenna_heading = self.orientation[2, 0] + self.dwell_centre[0, 0]
+            # Set rotation offset of underlying measurement model
+            rot_offset = \
+                StateVector(
+                    [[self.orientation[0, 0]],
+                     [self.orientation[1, 0]],
+                     [antenna_heading]])
+            tmp_meas_model = CartesianToBearingRange(
+                ndim_state=self.ndim_state,
+                mapping=self.position_mapping,
+                noise_covar=self.noise_covar,
+                translation_offset=self.position,
+                rotation_offset=rot_offset
+            )
+        else:
+            tmp_meas_model = CartesianToBearingRange(
+                ndim_state=measurement_model.ndim_state,
+                mapping=measurement_model.mapping,
+                noise_covar=measurement_model.noise_covar,
+                translation_offset=measurement_model.translation_offset,
+                rotation_offset=measurement_model.rotation_offset
+            )
         measurement_vector = tmp_meas_model.function(state, noise=False)
 
         # Check if state falls within sensor's FOV
@@ -322,8 +344,10 @@ def measurement_model(self):
             translation_offset=self.position,
             rotation_offset=self.orientation)
 
-    def is_detectable(self, state: GroundTruthState) -> bool:
-        measurement_vector = self.measurement_model.function(state, noise=False)
+    def is_detectable(self, state: GroundTruthState, measurement_model=None) -> bool:
+        if measurement_model is None:
+            measurement_model = self.measurement_model
+        measurement_vector = measurement_model.function(state, noise=False)
         true_range = measurement_vector[2, 0]  # Elevation(0), Bearing(1), Range(2)
         return true_range <= self.max_range
 
@@ -363,8 +387,10 @@ def measurement_model(self):
             velocity=self.velocity,
             rotation_offset=self.orientation)
 
-    def is_detectable(self, state: GroundTruthState) -> bool:
-        measurement_vector = self.measurement_model.function(state, noise=False)
+    def is_detectable(self, state: GroundTruthState, measurement_model=None) -> bool:
+        if measurement_model is None:
+            measurement_model = self.measurement_model
+        measurement_vector = measurement_model.function(state, noise=False)
         true_range = measurement_vector[1, 0]  # Bearing(0), Range(1), Range-Rate(2)
         return true_range <= self.max_range
 
@@ -403,8 +429,10 @@ def measurement_model(self):
             velocity=self.velocity,
             rotation_offset=self.orientation)
 
-    def is_detectable(self, state: GroundTruthState) -> bool:
-        measurement_vector = self.measurement_model.function(state, noise=False)
+    def is_detectable(self, state: GroundTruthState, measurement_model=None) -> bool:
+        if measurement_model is None:
+            measurement_model = self.measurement_model
+        measurement_vector = measurement_model.function(state, noise=False)
         true_range = measurement_vector[2, 0]  # Elevation(0), Bearing(1), Range(2), Range-Rate(3)
         return true_range <= self.max_range
 

stonesoup/sensor/radar/tests/test_radar.py

@@ -119,6 +119,8 @@ def test_simple_radar(h, sensorclass, ndim_state, pos_mapping, noise_covar, posi
 
     truth = {target_truth}
 
+    assert radar.is_detectable(target_truth)
+
     # Generate a noiseless measurement for the given target
     measurement = radar.measure(truth, noise=False)
     measurement = next(iter(measurement))  # Get measurement from set
@@ -146,6 +148,7 @@ def test_simple_radar(h, sensorclass, ndim_state, pos_mapping, noise_covar, posi
 
     # Generate a noiseless measurement for each of the given target states
     measurements = radar.measure(truth)
+    assert all(radar.is_detectable(t) for t in truth)
 
     # Two measurements for 2 truth states
     assert len(measurements) == 2
@@ -165,6 +168,7 @@ def test_simple_radar(h, sensorclass, ndim_state, pos_mapping, noise_covar, posi
 
     # Check no detection have been made when target is out of range
     assert (len(measurement3) == 0)
+    assert not radar.is_detectable(target3_truth)
 
 
 def h2d_rr(state, pos_map, vel_map, translation_offset, rotation_offset, velocity):
@@ -259,6 +263,8 @@ def test_range_rate_radar(h, sensorclass, pos_mapping, vel_mapping, noise_covar,
     target_truth = GroundTruthPath([target_state])
     truth = {target_truth}
 
+    assert radar.is_detectable(target_truth)
+
     # Generate a noiseless measurement for the given target
     measurement = radar.measure(truth, noise=False)
     measurement = next(iter(measurement))  # Get measurement from set
@@ -288,6 +294,7 @@ def test_range_rate_radar(h, sensorclass, pos_mapping, vel_mapping, noise_covar,
 
     # Two measurements for 2 truth states
     assert len(measurements) == 2
+    assert all(radar.is_detectable(t) for t in truth)
 
     # Measurements store ground truth paths
     for measurement in measurements:
@@ -391,6 +398,7 @@ def test_rotating_radar(sensorclass, radar_position, radar_orientation, state,
 
     # Assert no measurements since target is not in FOV
     assert len(measurement) == 0
+    assert not radar.is_detectable(target_truth)
 
     # Rotate radar such that the target is in FOV
     timestamp = timestamp + datetime.timedelta(seconds=0.5)
@@ -417,6 +425,7 @@ def test_rotating_radar(sensorclass, radar_position, radar_orientation, state,
         assert (np.equal(measurement.state_vector, eval_m[0]).all())
     else:
         assert (np.equal(measurement.state_vector, eval_m).all())
+    assert radar.is_detectable(target_truth)
 
     # Assert is TrueDetection type
     assert isinstance(measurement, TrueDetection)
@@ -433,6 +442,7 @@ def test_rotating_radar(sensorclass, radar_position, radar_orientation, state,
 
     # Two measurements for 2 truth states
     assert len(measurements) == 2
+    assert all(radar.is_detectable(t) for t in truth)
 
     # Measurements store ground truth paths
     for measurement in measurements:
@@ -491,6 +501,7 @@ def test_raster_scan_radar():
 
     # Assert no measurements since target is not in FOV
     assert len(measurement) == 0
+    assert not radar.is_detectable(target_truth)
 
     # Rotate radar
     timestamp = timestamp + datetime.timedelta(seconds=0.5)
@@ -505,6 +516,7 @@ def test_raster_scan_radar():
 
     # Assert no measurements since target is not in FOV
     assert len(measurement) == 0
+    assert not radar.is_detectable(target_truth)
 
     # Rotate radar such that the target is in FOV
     timestamp = timestamp + datetime.timedelta(seconds=1.0)
@@ -528,6 +540,7 @@ def test_raster_scan_radar():
     # Assert correction of generated measurement
     assert measurement.timestamp == target_state.timestamp
     assert np.array_equal(measurement.state_vector, eval_m)
+    assert radar.is_detectable(target_truth)
 
     # Assert is TrueDetection type
     assert isinstance(measurement, TrueDetection)

stonesoup/sensor/sensor.py

@@ -88,7 +88,7 @@ def measure(self, ground_truths: Set[GroundTruthState], noise: Union[np.ndarray,
         measurement_model = self.measurement_model
 
         detectable_ground_truths = [truth for truth in ground_truths
-                                    if self.is_detectable(truth)]
+                                    if self.is_detectable(truth, measurement_model)]
 
         if noise is True:
             if len(detectable_ground_truths) > 1:
@@ -126,7 +126,7 @@ def measure(self, ground_truths: Set[GroundTruthState], noise: Union[np.ndarray,
         return detections
 
     @abstractmethod
-    def is_detectable(self, state: GroundTruthState) -> bool:
+    def is_detectable(self, state: GroundTruthState, measurement_model=None) -> bool:
         raise NotImplementedError
 
     @abstractmethod