Add max range to simple Radars

stonesoup/sensor/radar/radar.py

@@ -51,6 +51,9 @@ class RadarBearingRange(Sensor):
         doc="An optional clutter generator that adds a set of simulated "
             ":class:`Clutter` ojects to the measurements at each time step. "
             "The clutter is simulated according to the provided distribution.")
+    max_range: float = Property(
+        default=np.inf,
+        doc="The maximum detection range of the radar (in meters)")
 
     @property
     def measurement_model(self):
@@ -68,7 +71,21 @@ def measure(self, ground_truths: Set[GroundTruthState], noise: Union[np.ndarray,
 
         detections = set()
         for truth in ground_truths:
-            measurement_vector = measurement_model.function(truth, noise=noise, **kwargs)
+            measurement_vector = measurement_model.function(truth, noise=False, **kwargs)
+
+            if noise is True:
+                measurement_noise = measurement_model.rvs()
+            else:
+                measurement_noise = noise
+
+            range_t = measurement_vector[1, 0]  # Bearing(0), Range(1)
+            # Do not measure if state is out of range
+            if range_t > self.max_range:
+                continue
+
+            # Add in measurement noise to the measurement vector
+            measurement_vector += measurement_noise
+
             detection = TrueDetection(measurement_vector,
                                       measurement_model=measurement_model,
                                       timestamp=truth.timestamp,
@@ -102,11 +119,14 @@ class RadarRotatingBearingRange(RadarBearingRange):
             "sensor frame/orientation. The angle is positive if the rotation is in the "
             "counter-clockwise direction when viewed by an observer looking down the z-axis of "
             "the sensor frame, towards the origin. Angle units are in radians",
-        generator_cls=DwellActionsGenerator
-    )
-    rpm: float = Property(doc="The number of antenna rotations per minute (RPM)")
-    max_range: float = Property(doc="The maximum detection range of the radar (in meters)")
-    fov_angle: float = Property(doc="The radar field of view (FOV) angle (in radians).")
+        generator_cls=DwellActionsGenerator)
+    rpm: float = Property(
+        doc="The number of antenna rotations per minute (RPM)")
+    max_range: float = Property(
+        default=np.inf,
+        doc="The maximum detection range of the radar (in meters)")
+    fov_angle: float = Property(
+        doc="The radar field of view (FOV) angle (in radians).")
 
     @property
     def measurement_model(self):
@@ -189,6 +209,9 @@ class RadarElevationBearingRange(RadarBearingRange):
         doc="The sensor noise covariance matrix. This is utilised by "
             "(and follow in format) the underlying "
             ":class:`~.CartesianToElevationBearingRange` model")
+    max_range: float = Property(
+        default=np.inf,
+        doc="The maximum detection range of the radar (in meters)")
 
     @property
     def measurement_model(self):
@@ -206,7 +229,23 @@ def measure(self, ground_truths: Set[GroundTruthState], noise: Union[np.ndarray,
 
         detections = set()
         for truth in ground_truths:
-            measurement_vector = measurement_model.function(truth, noise=noise, **kwargs)
+            # Initially no noise is added to the measurement vector
+            measurement_vector = measurement_model.function(truth, noise=False, **kwargs)
+
+            if noise is True:
+                measurement_noise = measurement_model.rvs()
+            else:
+                measurement_noise = noise
+
+            # Check if state falls within range of sensor
+            range_t = measurement_vector[2, 0]  # Elevation(0), Bearing(1), Range(2)
+            # Do not measure if state is out of range
+            if range_t > self.max_range:
+                continue
+
+            # Add in measurement noise to the measurement vector
+            measurement_vector += measurement_noise
+
             detection = TrueDetection(measurement_vector,
                                       measurement_model=measurement_model,
                                       timestamp=truth.timestamp,

stonesoup/sensor/radar/tests/test_radar.py

@@ -60,7 +60,7 @@ def h3d(state, pos_map, translation_offset, rotation_offset):
 
 
 @pytest.mark.parametrize(
-    "h, sensorclass, ndim_state, pos_mapping, noise_covar, position, target",
+    "h, sensorclass, ndim_state, pos_mapping, noise_covar, position, target, max_range",
     [
         (
                 h2d,  # h
@@ -70,7 +70,8 @@ def h3d(state, pos_map, translation_offset, rotation_offset):
                 np.array([[0.015, 0],
                           [0, 0.1]]),  # noise_covar
                 StateVector([[1], [1]]),  # position
-                np.array([[200], [10]])  # target
+                np.array([[200], [10]]),  # target
+                1000  # max_range
         ),
         (
                 h3d,  # h
@@ -81,17 +82,20 @@ def h3d(state, pos_map, translation_offset, rotation_offset):
                           [0, 0.015, 0],
                           [0, 0, 0.1]]),  # noise_covar
                 StateVector([[1], [1], [0]]),  # position
-                np.array([[200], [10], [10]])  # target
+                np.array([[200], [10], [10]]),  # target
+                1000  # max_range
         )
     ],
     ids=["RadarBearingRange", "RadarElevationBearingRange"]
 )
-def test_simple_radar(h, sensorclass, ndim_state, pos_mapping, noise_covar, position, target):
-    # Instantiate the rotating radar
+def test_simple_radar(h, sensorclass, ndim_state, pos_mapping, noise_covar, position, target,
+                      max_range):
+    # Instantiate the simple radar
     radar = sensorclass(ndim_state=ndim_state,
                         position_mapping=pos_mapping,
                         noise_covar=noise_covar,
-                        position=position)
+                        position=position,
+                        max_range=max_range)
 
     assert (np.equal(radar.position, position).all())
 
@@ -132,6 +136,17 @@ def test_simple_radar(h, sensorclass, ndim_state, pos_mapping, noise_covar, posi
         assert measurement.groundtruth_path in truth
         assert isinstance(measurement.groundtruth_path, GroundTruthPath)
 
+    # Assert that no detection is made when target is out of range
+    target3 = np.array([[2_000], [20], [20]])
+    target3_state = GroundTruthState(target3, timestamp=datetime.datetime.now())
+    target3_truth = GroundTruthPath([target3_state])
+
+    # Don't want any noise since we're range testing
+    measurement3 = radar.measure(target3_truth, noise=False)
+
+    # Check no detection have been made when target is out of range
+    assert (len(measurement3) == 0)
+
 
 def h2d_rr(state, pos_map, vel_map, translation_offset, rotation_offset, velocity):
     xyz = StateVector([[state.state_vector[pos_map[0], 0] - translation_offset[0, 0]],