diff --git a/stonesoup/sensor/passive.py b/stonesoup/sensor/passive.py index ec428bd97..1bee8f3fa 100644 --- a/stonesoup/sensor/passive.py +++ 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: diff --git a/stonesoup/sensor/radar/radar.py b/stonesoup/sensor/radar/radar.py index 3ad171ca6..65fb1c0b8 100644 --- a/stonesoup/sensor/radar/radar.py +++ b/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 diff --git a/stonesoup/sensor/sensor.py b/stonesoup/sensor/sensor.py index baa2a8546..748506257 100644 --- a/stonesoup/sensor/sensor.py +++ b/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