From 02e15114fb83154996ab5830451055846e085514 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Mon, 30 Mar 2020 17:40:06 +0100 Subject: [PATCH 01/51] Update to platform to allow it to control the positions of its sensors, rather than saving it to the sensor each time the platform moves. --- stonesoup/platform/base.py | 12 ++ stonesoup/platform/simple.py | 204 +++++++++++------- .../platform/tests/test_platform_simple.py | 73 +++++-- stonesoup/sensor/radar/radar.py | 21 +- stonesoup/sensor/radar/tests/test_radar.py | 16 +- 5 files changed, 217 insertions(+), 109 deletions(-) diff --git a/stonesoup/platform/base.py b/stonesoup/platform/base.py index f4e324e04..bdc057946 100644 --- a/stonesoup/platform/base.py +++ b/stonesoup/platform/base.py @@ -16,6 +16,18 @@ class Platform(Base): transition_model = Property( TransitionModel, doc="Transition model") + def get_position(self): + if self.state.ndim == 6 or self.state.ndim == 4: + return self.state.state_vector[0::2] + else: + raise NotImplementedError + + def get_velocity(self): + if self.state.ndim == 6 or self.state.ndim == 4: + return self.state.state_vector[1::2] + else: + raise NotImplementedError + def move(self, timestamp=None, **kwargs): """Propagate the platform position using the :attr:`transition_model`. diff --git a/stonesoup/platform/simple.py b/stonesoup/platform/simple.py index e33fb5a3c..765d0ce30 100644 --- a/stonesoup/platform/simple.py +++ b/stonesoup/platform/simple.py @@ -2,14 +2,39 @@ import numpy as np from math import cos, sin from scipy.linalg import expm +import weakref +from functools import lru_cache +from stonesoup.sensor.base import Sensor3DCartesian from ..base import Property from ..types.state import StateVector -from ..sensor import Sensor from ..functions import cart2pol from .base import Platform +class PlatformSensor(Sensor3DCartesian): + platform_system = Property(Platform, default=None, + doc='`weakref` to the platform on which the ' + 'sensor is mounted') + + def measure(self, **kwargs): + raise NotImplementedError + + def get_position(self): + if (hasattr(self, 'platform_system') + and self.platform_system is not None): + return self.platform_system().get_sensor_position(self) + else: + return self.position + + def get_orientation(self): + if (hasattr(self, 'platform_system') + and self.platform_system is not None): + return self.platform_system().get_sensor_orientation(self) + else: + return self.orientation + + class SensorPlatform(Platform): """A simple Platform that can carry a number of different sensors and is capable of moving based upon the :class:`~.TransitionModel`. @@ -24,15 +49,15 @@ class SensorPlatform(Platform): """ - sensors = Property([Sensor], doc="A list of N mounted sensors") + sensors = Property([PlatformSensor], doc="A list of N mounted sensors") mounting_offsets = Property( [np.array], doc="A list of sensor offsets (For now expressed\ as a Nxn array of nD Cartesian coordinates)") mounting_mappings = Property( [np.array], doc="Mappings between the platform state vector and the\ individuals sensors mounting offset (For now\ - expressed as a nxN array of nD Cartesian\ - coordinates or a 1xN array where a single\ + expressed as a Nxn array of nD Cartesian\ + coordinates or a 1xn array where a single\ mapping will be applied to all sensors)") # TODO: Determine where a platform coordinate frame should be maintained @@ -67,66 +92,82 @@ def __init__(self, *args, **kwargs): self.mounting_mappings, axis=0) self.mounting_mappings = mapping_array + for sensor in self.sensors: + sensor.platform_system = weakref.ref(self) + sensor.position = None + sensor.orientation = None - self._move_sensors() - - # TODO: create add_sensor method - - def move(self, timestamp=None, **kwargs): - """Propagate the platform position using the :attr:`transition_model`, - and use _move_sensors method to update sensor positions, this in turn - calls _get_rotated_offset to modify sensor offsets relative to the - platforms velocity vector - - Parameters - ---------- - timestamp: :class:`datetime.datetime`, optional - A timestamp signifying when the maneuver completes - (the default is `None`) - - Notes - ----- - This methods updates the value of :attr:`position` and :attr:`sensors` - """ - # Call superclass method to update platform state - super().move(timestamp=timestamp, **kwargs) - # Move the platforms sensors relative to the platform - self._move_sensors() - - def _move_sensors(self): - """ Propogate the Sensor positions based upon the mounting - offsets and the platform position and heading post manoeuvre. - - Notes - ----- - Method assumes that if a platform has a transition model it will have - velocity components. A sensor offset will therefore be rotated based - upon the platforms velocity (i.e. direction of motion) - """ + def add_sensor(self, sensor, mounting_offset, mounting_mapping=None): + """ Determine the sensor mounting offset for the platforms relative + orientation. + + Parameters + ---------- + sensor : :class:`Sensor` + The sensor object to add + mounting_offset : :class:`np.ndarray` + A 1xN array with the mounting offset of the new sensor + mounting_mapping : :class:`np.ndarray`, optional + A 1xN array with the mounting mapping of the new sensor. + If `None` (default) then use the same mapping as all + previous sensors. If all sensor do not a have the same + mapping then raise ValueError + """ + self.sensors.append(sensor) + sensor.platform_system = weakref.ref(self) + sensor.position = None + sensor.orientation = None + + if mounting_mapping is None: + if not np.all(self.mounting_mappings + == self.mounting_mappings[0, :]): + raise ValueError('Mapping must be specified unless all ' + 'sensors have the same mapping') + mounting_mapping = self.mounting_mappings[0, :] + + if mounting_offset.ndim == 1: + mounting_offset = mounting_offset[np.newaxis, :] + if mounting_mapping.ndim == 1: + mounting_mapping = mounting_mapping[np.newaxis, :] + + if len(self.sensors) == 1: + # This is the first sensor added, so no mounting mappings/offsets + # to maintain + self.mounting_offsets = mounting_offset + else: + self.mounting_offsets = np.concatenate([self.mounting_offsets, + mounting_offset]) + + if len(self.sensors) == 1: + # This is the first sensor added, so no mounting + # mappings/offsets to maintain + self.mounting_mappings = mounting_mapping + else: + self.mounting_mappings = np.concatenate([self.mounting_mappings, + mounting_mapping]) + + def get_sensor_position(self, sensor): + i = self.sensors.index(sensor) + if self.is_moving(): + offsets = self._get_rotated_offset(i) + else: + offsets = StateVector(self.mounting_offsets[i, :]) + new_sensor_pos = self.get_position() + offsets + return StateVector(new_sensor_pos) + + def get_sensor_orientation(self, sensor): + i = self.sensors.index(sensor) + vel = np.zeros([self.mounting_mappings.shape[1], 1]) + for j in range(self.mounting_mappings.shape[1]): + vel[j, 0] = self.state.state_vector[ + self.mounting_mappings[i, j] + 1] + abs_vel, heading = cart2pol(vel[0, 0], vel[1, 0]) + return StateVector([[0], [0], [heading]]) - # Update the positions of all sensors relative to the platform - for i in range(len(self.sensors)): - if (hasattr(self, 'transition_model') & - (np.absolute(self.state.state_vector[ - self.mounting_mappings[0]+1]).max() > 0)): - new_sensor_pos = self._get_rotated_offset(i) - for j in range(self.mounting_offsets.shape[1]): - new_sensor_pos[j] = new_sensor_pos[j] + \ - (self.state.state_vector[ - self.mounting_mappings[i, j]]) - else: - new_sensor_pos = np.zeros([self.mounting_offsets.shape[1], 1]) - for j in range(self.mounting_offsets.shape[1]): - new_sensor_pos[j] = (self.state.state_vector[ - self.mounting_mappings[i, j]] + - self.mounting_offsets[i, j]) - self.sensors[i].position = (StateVector(new_sensor_pos)) - vel = np.zeros([self.mounting_mappings.shape[1], 1]) - for j in range(self.mounting_mappings.shape[1]): - vel[j, 0] = self.state.state_vector[ - self.mounting_mappings[i, j] + 1] - abs_vel, heading = cart2pol(vel[0, 0], vel[1, 0]) - self.sensors[i].orientation = (StateVector([[0], [0], [heading]])) + def is_moving(self): + return (hasattr(self, 'transition_model') + and self.transition_model is not None + and np.any(self.get_velocity() != 0)) def _get_rotated_offset(self, i): """ Determine the sensor mounting offset for the platforms relative @@ -139,14 +180,11 @@ def _get_rotated_offset(self, i): Returns ------- - np.array + np.ndarray Sensor mounting offset rotated relative to platform motion """ - vel = np.zeros([self.mounting_mappings.shape[1], 1]) - for j in range(self.mounting_mappings.shape[1]): - vel[j, 0] = self.state.state_vector[ - self.mounting_mappings[i, j] + 1] + vel = self.get_velocity() rot = _get_rotation_matrix(vel) return np.transpose(np.dot(rot, self.mounting_offsets[i])[np.newaxis]) @@ -165,7 +203,7 @@ def _get_rotation_matrix(vel): Parameters ---------- - vel : np.arrary + vel : np.ndarrary 1xD vector denoting platform velocity in D dimensions Returns @@ -190,9 +228,9 @@ def _get_angle(vec, axis): Parameters ---------- - vec : np.array + vec : np.ndarray 1xD array denoting platform velocity - axis : np.array + axis : np.ndarray Dx1 array denoting sensor offset relative to platform Returns @@ -220,18 +258,32 @@ def _rot3d(vec): Parameters ---------- - vec: np.array + vec: np.ndarray platform velocity Returns ------- - np.array + np.ndarray 3x3 rotation matrix + """ + return _rot3d_tuple(tuple(v[0] for v in vec)) + + +@lru_cache(maxsize=128) +def _rot3d_tuple(vec): + """ Private method. Should not be called directly, only from `_rot3d` + + Params and returns as :func:`~_rot3d` + + This wrapped method takes a tuple rather than a state vector. This allows caching, which + is important as the new sensor approach means `_rot3d` is called on each call to get_position, + and becomes a significant performance hit. + """ # TODO handle platform roll - yaw = np.arctan2(vec[[1]], vec[[0]]) - pitch = np.arctan2(vec[[2]], - np.sqrt(vec[[0]] ** 2 + vec[[1]] ** 2)) * -1 + yaw = np.arctan2(vec[1], vec[0]) + pitch = np.arctan2(vec[2], + np.sqrt(vec[0] ** 2 + vec[1] ** 2)) * -1 rot_z = _rot_z(yaw) # Modify to correct for new y axis y_axis = np.array([0, 1, 0]) @@ -251,7 +303,7 @@ def _rot_z(theta): Returns ------- - np.array + np.ndarray 3x3 rotation matrix """ return np.array([[cos(theta), -sin(theta), 0], diff --git a/stonesoup/platform/tests/test_platform_simple.py b/stonesoup/platform/tests/test_platform_simple.py index ec24fe972..074d4a8f8 100644 --- a/stonesoup/platform/tests/test_platform_simple.py +++ b/stonesoup/platform/tests/test_platform_simple.py @@ -305,6 +305,16 @@ def move(request): return request.param +@pytest.fixture(params=[True, False], ids=["Add", "Initialise"]) +def add_sensor(request): + return request.param + + +@pytest.fixture(params=[True, False], ids=["MM_empty", "MM_added"]) +def mounting_mapping_on_add(request): + return request.param + + testdata_2d = [ np.array([[0], [0], [0], [0]]), np.array([[10], [0], [0], [0]]), @@ -354,7 +364,8 @@ def move(request): 'state, expected', zip(testdata_2d, expected_2d), ids=["Static", "pos offset", "x vel", "y vel", "-x vel", "-y vel", "x,y vel", "-x,-y vel", "x,-y vel", "-x,y vel"]) -def test_2d_platform(state, expected, move, radars_2d, mounting_offsets_2d): +def test_2d_platform(state, expected, move, radars_2d, + mounting_offsets_2d, add_sensor, mounting_mapping_on_add): # Define time related variables timestamp = datetime.datetime.now() # Define transition model and position for platform @@ -366,13 +377,28 @@ def test_2d_platform(state, expected, move, radars_2d, mounting_offsets_2d): # This defines the mapping to the platforms state vector (i.e. x and y) mounting_mappings = np.array([[0, 2]]) # create a platform with the simple radar mounted - platform = SensorPlatform( - state=platform_state, - transition_model=trans_model, - sensors=radars_2d, - mounting_offsets=mounting_offsets_2d, - mounting_mappings=mounting_mappings - ) + if add_sensor: + platform = SensorPlatform( + state=platform_state, + transition_model=trans_model, + sensors=[], + mounting_offsets=np.array([]), + mounting_mappings=mounting_mappings + ) + for i, sensor in enumerate(radars_2d): + if mounting_mapping_on_add: + platform.add_sensor(sensor, mounting_offsets_2d[i, :], + mounting_mappings) + else: + platform.add_sensor(sensor, mounting_offsets_2d[i, :]) + else: + platform = SensorPlatform( + state=platform_state, + transition_model=trans_model, + sensors=radars_2d, + mounting_offsets=mounting_offsets_2d, + mounting_mappings=mounting_mappings + ) if move: # Move the platform platform.move(timestamp + datetime.timedelta(seconds=2)) @@ -423,13 +449,28 @@ def test_3d_platform(state, expected, move, radars_3d, mounting_offsets_3d): # This defines the mapping to the platforms state vector (i.e. x and y) mounting_mappings = np.array([[0, 2, 4]]) # create a platform with the simple radar mounted - platform = SensorPlatform( - state=platform_state, - transition_model=trans_model, - sensors=radars_3d, - mounting_offsets=mounting_offsets_3d, - mounting_mappings=mounting_mappings - ) + if add_sensor: + platform = SensorPlatform( + state=platform_state, + transition_model=trans_model, + sensors=[], + mounting_offsets=np.array([]), + mounting_mappings=mounting_mappings + ) + for i, sensor in enumerate(radars_3d): + if mounting_mapping_on_add: + platform.add_sensor(sensor, mounting_offsets_3d[i, :], + mounting_mappings) + else: + platform.add_sensor(sensor, mounting_offsets_3d[i, :]) + else: + platform = SensorPlatform( + state=platform_state, + transition_model=trans_model, + sensors=radars_3d, + mounting_offsets=mounting_offsets_3d, + mounting_mappings=mounting_mappings + ) if move: # Move the platform platform.move(timestamp + datetime.timedelta(seconds=2)) @@ -450,7 +491,7 @@ def sensor_positions_test(expected_offset, platform): [len(platform.sensors), platform.mounting_offsets.shape[1]]) expected_radar_position = np.zeros_like(radar_position) for i in range(len(platform.sensors)): - radar_position[i, :] = platform.sensors[i].position.flatten() + radar_position[i, :] = platform.sensors[i].get_position().flatten() platform_position = platform.state.state_vector[ platform.mounting_mappings[i, :]].flatten() diff --git a/stonesoup/sensor/radar/radar.py b/stonesoup/sensor/radar/radar.py index a4290a4e8..8f00bbd86 100644 --- a/stonesoup/sensor/radar/radar.py +++ b/stonesoup/sensor/radar/radar.py @@ -2,9 +2,10 @@ import numpy as np from math import erfc + +from ...platform.simple import PlatformSensor from ...functions import cart2sphere, rotx, roty, rotz -from ..base import Sensor -from ..base import Property, Sensor3DCartesian +from ..base import Property from ...models.measurement.nonlinear import CartesianToBearingRange from ...types.array import CovarianceMatrix @@ -17,7 +18,7 @@ import scipy.constants as const -class RadarRangeBearing(Sensor3DCartesian): +class RadarRangeBearing(PlatformSensor): """A simple radar sensor that generates measurements of targets, using a :class:`~.CartesianToBearingRange` model, relative to its position. @@ -64,7 +65,7 @@ def measure(self, ground_truth, noise=True, **kwargs): ndim_state=self.ndim_state, mapping=self.mapping, noise_covar=self.noise_covar, - translation_offset=self.position, + translation_offset=self.get_position(), rotation_offset=self.orientation) measurement_vector = measurement_model.function( @@ -255,7 +256,7 @@ def rotate(self, timestamp): self.rpm = -self.rpm -class AESARadar(Sensor): +class AESARadar(PlatformSensor): r"""An AESA (Active electronically scanned array) radar model that calculates the signal to noise ratio (SNR) of a target and the subsequent probability of detection (PD). The SNR is calculated using: @@ -305,16 +306,13 @@ class AESARadar(Sensor): The current implementation of this class assumes a 3D Cartesian plane. This model does not generate false alarms. """ - rotation_offset = Property( StateVector, default=StateVector([0, 0, 0]), doc="A 3x1 array of angles (rad), specifying " "the radar orientation in terms of the " "counter-clockwise rotation around the " ":math:`x,y,z` axis. i.e Roll, Pitch and Yaw.") - translation_offset = Property( - StateVector, default=StateVector([0, 0, 0]), - doc="The radar position in 3D Cartesian space.[x,y,z]") + mapping = Property( np.array, default=[0, 1, 2], doc="Mapping between or positions and state " @@ -444,8 +442,7 @@ def gen_probability(self, sky_state): spoiled_gain = 10 ** (self.antenna_gain / 10) * np.cos(beam_az) * np.cos(beam_el) spoiled_width = self.beam_width / (np.cos(beam_az) * np.cos(beam_el)) # state relative to radar (in cartesian space) - relative_vector = sky_state.state_vector[self.mapping] \ - - self.translation_offset[self.mapping] # noqa E127 + relative_vector = sky_state.state_vector[self.mapping] - self.get_position() relative_vector = self._rotation_matrix @ relative_vector # calculate target position in spherical coordinates @@ -489,7 +486,7 @@ def measure(self, sky_state, noise=True, **kwargs): det_prob = self.gen_probability(sky_state)[0] # Is the state detected? if np.random.rand() <= det_prob: - self.measurement_model.translation_offset = self.translation_offset + self.measurement_model.translation_offset = self.get_position() self.measurement_model.rotation_offset = self.rotation_offset measured_pos = self.measurement_model.function(sky_state, noise=noise) diff --git a/stonesoup/sensor/radar/tests/test_radar.py b/stonesoup/sensor/radar/tests/test_radar.py index cc4d5e00e..84003ca80 100644 --- a/stonesoup/sensor/radar/tests/test_radar.py +++ b/stonesoup/sensor/radar/tests/test_radar.py @@ -234,7 +234,8 @@ def test_aesaradar(): radar = AESARadar(antenna_gain=30, mapping=[0, 2, 4], - translation_offset=StateVector([0.0] * 6), + position=StateVector([0.0] * 3), + orientation=StateVector([0.0] * 3), frequency=100e6, number_pulses=5, duty_cycle=0.1, @@ -279,7 +280,9 @@ def test_swer(repeats=10000): beam_shape=Beam2DGaussian(peak_power=50e3), beam_transition_model=StationaryBeam( centre=[np.deg2rad(15), np.deg2rad(20)]), - measurement_model=None) + measurement_model=None, + position=StateVector([0.0]*3), + orientation=StateVector([0.0]*3)) # populate list of random rcs for i in range(0, repeats): list_rcs[i] = radar.gen_probability(target)[2] @@ -294,7 +297,8 @@ def test_swer(repeats=10000): def test_detection(): radar = AESARadar(antenna_gain=30, - translation_offset=StateVector([0.0] * 3), + position=StateVector([0.0] * 3), + orientation=StateVector([0.0] * 3), frequency=100e6, number_pulses=5, duty_cycle=0.1, @@ -325,7 +329,8 @@ def test_failed_detect(): radar = AESARadar(antenna_gain=30, mapping=[0, 2, 4], - translation_offset=StateVector([0.0] * 6), + position=StateVector([0.0] * 3), + orientation=StateVector([0.0] * 3), frequency=100e6, number_pulses=5, duty_cycle=0.1, @@ -355,7 +360,8 @@ def test_target_rcs(): radar_model = AESARadar(antenna_gain=36, mapping=[0, 1, 2], - translation_offset=StateVector([0.0]*3), + position=StateVector([0.0]*3), + orientation=StateVector([0.0] * 3), frequency=10e9, number_pulses=10, duty_cycle=0.18, From ed62b501724e9af443b6de61092c4d45ebae624f Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Fri, 3 Apr 2020 16:00:19 +0100 Subject: [PATCH 02/51] Multiple changes to address pull request comments: - Create both fixed and moving sensor platforms - Force a sensor to be mounted to a platform - properties for position and orientation - Use a mapping between state_vector and position - Give Platform a state_vector property - Force mappings to be sensible shape - Update tests to match Still needs some tidying and documentation --- stonesoup/functions.py | 26 +- stonesoup/models/measurement/base.py | 14 +- stonesoup/models/measurement/nonlinear.py | 15 +- stonesoup/platform/base.py | 126 ++++++++-- stonesoup/platform/simple.py | 230 +++++++++--------- .../platform/tests/test_platform_base.py | 10 +- .../platform/tests/test_platform_simple.py | 201 ++++++--------- stonesoup/sensor/radar/radar.py | 6 +- stonesoup/sensor/radar/tests/test_radar.py | 121 +++++---- 9 files changed, 418 insertions(+), 331 deletions(-) diff --git a/stonesoup/functions.py b/stonesoup/functions.py index db7567f27..b3b8acee4 100644 --- a/stonesoup/functions.py +++ b/stonesoup/functions.py @@ -1,11 +1,12 @@ # -*- coding: utf-8 -*- """Mathematical functions used within Stone Soup""" +from typing import List, Union import numpy as np from copy import copy from .types.numeric import Probability -from .types.array import Matrix +from .types.array import Matrix, StateVector def tria(matrix): @@ -566,3 +567,26 @@ def mod_elevation(x): elif N == 3: x = x - 2.0 * np.pi return x + + +def coerce_to_valid_mapping(mapping: Union[List[int], np.ndarray, StateVector]): + """Function to take a mapping and convert it to a suibale form for indexing a + :class:`StateVector`. Needed because if you index a :class:`StateVector` with another + :class:`StateVector` (which would be the obvious technique) you get a 3d :class:`ndarray`. + + The input as either a List, :class:`ndarray` or a :class:`StateVector`, all of which should + contain integers. It returns a 1d :class:`ndarray` of with `shape == (ndim,)` where `ndim` is + the number of elements in the mapping array. + + Parameters + ---------- + mapping: :class:`Union[List, np.ndarray]` + A mapping that needs to be formatted to a valid mapping shape + + Returns + ------- + mapping: :class:`np.ndarray` + A flattened np.ndarray suitable for indexing a :class:`StateVector` + """ + + return np.asarray(mapping).flatten() diff --git a/stonesoup/models/measurement/base.py b/stonesoup/models/measurement/base.py index 65aa09322..d9329fc96 100644 --- a/stonesoup/models/measurement/base.py +++ b/stonesoup/models/measurement/base.py @@ -1,18 +1,22 @@ # -*- coding: utf-8 -*- -from abc import abstractmethod +from abc import abstractmethod, ABC -import scipy as sp +import numpy as np +from stonesoup.functions import coerce_to_valid_mapping from ..base import Model from ...base import Property -class MeasurementModel(Model): +class MeasurementModel(Model, ABC): """Measurement Model base class""" ndim_state = Property(int, doc="Number of state dimensions") - mapping = Property( - sp.ndarray, doc="Mapping between measurement and state dims") + mapping = Property(np.ndarray, doc="Mapping between measurement and state dims") + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self.mapping = coerce_to_valid_mapping(self.mapping) @property def ndim(self): diff --git a/stonesoup/models/measurement/nonlinear.py b/stonesoup/models/measurement/nonlinear.py index 6dd634cb1..d449022d7 100644 --- a/stonesoup/models/measurement/nonlinear.py +++ b/stonesoup/models/measurement/nonlinear.py @@ -1,4 +1,6 @@ # -*- coding: utf-8 -*- +from abc import ABC +from typing import List import copy import numpy as np @@ -15,8 +17,7 @@ from .base import MeasurementModel -class CombinedReversibleGaussianMeasurementModel( - ReversibleModel, GaussianModel, MeasurementModel): +class CombinedReversibleGaussianMeasurementModel(ReversibleModel, GaussianModel, MeasurementModel): r"""Combine multiple models into a single model by stacking them. The assumption is that all models are Gaussian, and must be combination of @@ -29,8 +30,7 @@ class CombinedReversibleGaussianMeasurementModel( :class:`~.LinearModel` or :class:`~.ReversibleModel`. """ mapping = None - model_list = Property( - [MeasurementModel], doc="List of Measurement Models.") + model_list = Property(List[MeasurementModel], doc="List of Measurement Models.") def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) @@ -90,9 +90,7 @@ def rvs(self, num_samples=1, **kwargs): return rvs_vectors.view(Matrix) -class NonLinearGaussianMeasurement(MeasurementModel, - NonLinearModel, - GaussianModel): +class NonLinearGaussianMeasurement(MeasurementModel, NonLinearModel, GaussianModel, ABC): r"""This class combines the MeasurementModel, NonLinearModel and \ GaussianModel classes. It is not meant to be instantiated directly \ but subclasses should be derived from this class. @@ -272,8 +270,7 @@ def rvs(self, num_samples=1, **kwargs): return out -class CartesianToBearingRange( - NonLinearGaussianMeasurement, ReversibleModel): +class CartesianToBearingRange(NonLinearGaussianMeasurement, ReversibleModel): r"""This is a class implementation of a time-invariant measurement model, \ where measurements are assumed to be received in the form of bearing \ (:math:`\phi`) and range (:math:`r`), with Gaussian noise in each dimension. diff --git a/stonesoup/platform/base.py b/stonesoup/platform/base.py index bdc057946..8efaab4ef 100644 --- a/stonesoup/platform/base.py +++ b/stonesoup/platform/base.py @@ -1,32 +1,125 @@ # -*- coding: utf-8 -*- +from abc import abstractmethod, ABC +from ..functions import cart2sphere, cart2pol, coerce_to_valid_mapping +from ..types.array import StateVector from ..base import Base, Property from ..types.state import State from ..models.transition import TransitionModel +import numpy as np -class Platform(Base): - """Platform base class +class Platform(Base, ABC): + state = Property(State, doc="The platform state at any given point") + mapping = Property(np.ndarray, doc="Mapping between platform position and state dims") + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self.mapping = coerce_to_valid_mapping(self.mapping) + + @property + def state_vector(self): + return self.state.state_vector + + @property + def position(self): + return self.state_vector[self.mapping] + + @property + def ndim(self): + return len(self.mapping) + + @property + @abstractmethod + def orientation(self): + raise NotImplementedError + + @property + @abstractmethod + def velocity(self): + raise NotImplementedError + + @property + @abstractmethod + def acceleration(self): + raise NotImplementedError + + @abstractmethod + def is_moving(self): + raise NotImplementedError + + @abstractmethod + def move(self, timestamp): + raise NotImplementedError + + +class FixedPlatform(Platform): + orientation = Property(StateVector, default=StateVector([0, 0, 0]), + doc='A fixed orientation of the static platform') + + @property + def velocity(self): + return StateVector([0] * self.ndim) + + @property + def acceleration(self): + return StateVector([0] * self.ndim) + + def is_moving(self): + return False + + def move(self, timestamp): + # Return without moving static platforms + self.state.timestamp = timestamp + + +class MovingPlatform(Platform): + """Moving platform base class A platform represents a random object defined as a :class:`~.State` that moves according to a given :class:`~.TransitionModel`. """ - - state = Property(State, doc="The platform state at any given point") transition_model = Property( TransitionModel, doc="Transition model") - def get_position(self): - if self.state.ndim == 6 or self.state.ndim == 4: - return self.state.state_vector[0::2] - else: - raise NotImplementedError + @property + def velocity(self): + # TODO docs + # TODO return zeros? + try: + return self.state_vector[self.mapping + 1] + except IndexError: + raise AttributeError('Velocity is not defined for this platform') + + @property + def acceleration(self): + # TODO docs + # TODO return zeros? + try: + return self.state_vector[self.mapping + 2] + except IndexError: + raise AttributeError('Acceleration is not defined for this platform') - def get_velocity(self): - if self.state.ndim == 6 or self.state.ndim == 4: - return self.state.state_vector[1::2] - else: - raise NotImplementedError + @property + def orientation(self): + # TODO docs + # TODO handle roll? + try: + velocity = self.velocity + except AttributeError: + raise AttributeError('Orientation of a zero-velocity moving platform is not defined') + if self.ndim == 3: + _, bearing, elevation = cart2sphere(*velocity) + return StateVector([0, bearing, elevation]) + elif self.ndim == 2: + _, bearing = cart2pol(*velocity) + return StateVector([0, bearing]) + + def is_moving(self): + # TODO docs + return (hasattr(self, 'transition_model') + and self.transition_model is not None + and np.any(self.velocity != 0)) def move(self, timestamp=None, **kwargs): """Propagate the platform position using the :attr:`transition_model`. @@ -54,11 +147,6 @@ def move(self, timestamp=None, **kwargs): # TypeError: (timestamp or prior.timestamp) is None time_interval = None - # Return without moving static platforms - if self.transition_model is None: - self.state.timestamp = timestamp - return self - self.state = State( state_vector=self.transition_model.function( state=self.state, diff --git a/stonesoup/platform/simple.py b/stonesoup/platform/simple.py index 765d0ce30..04f7a6e46 100644 --- a/stonesoup/platform/simple.py +++ b/stonesoup/platform/simple.py @@ -1,18 +1,22 @@ # -*- coding: utf-8 -*- +from abc import ABC +from collections.abc import Sequence +from typing import List, Union + import numpy as np from math import cos, sin from scipy.linalg import expm import weakref from functools import lru_cache -from stonesoup.sensor.base import Sensor3DCartesian +from ..functions import coerce_to_valid_mapping, rotz +from ..sensor.base import Sensor from ..base import Property from ..types.state import StateVector -from ..functions import cart2pol -from .base import Platform +from .base import Platform, MovingPlatform, FixedPlatform -class PlatformSensor(Sensor3DCartesian): +class PlatformSensor(Sensor): platform_system = Property(Platform, default=None, doc='`weakref` to the platform on which the ' 'sensor is mounted') @@ -20,22 +24,16 @@ class PlatformSensor(Sensor3DCartesian): def measure(self, **kwargs): raise NotImplementedError - def get_position(self): - if (hasattr(self, 'platform_system') - and self.platform_system is not None): - return self.platform_system().get_sensor_position(self) - else: - return self.position + @property + def position(self): + return self.platform_system().get_sensor_position(self) - def get_orientation(self): - if (hasattr(self, 'platform_system') - and self.platform_system is not None): - return self.platform_system().get_sensor_orientation(self) - else: - return self.orientation + @property + def orientation(self): + return self.platform_system().get_sensor_orientation(self) -class SensorPlatform(Platform): +class SensorPlatformMixin(Platform, ABC): """A simple Platform that can carry a number of different sensors and is capable of moving based upon the :class:`~.TransitionModel`. @@ -49,16 +47,22 @@ class SensorPlatform(Platform): """ - sensors = Property([PlatformSensor], doc="A list of N mounted sensors") - mounting_offsets = Property( - [np.array], doc="A list of sensor offsets (For now expressed\ - as a Nxn array of nD Cartesian coordinates)") - mounting_mappings = Property( - [np.array], doc="Mappings between the platform state vector and the\ - individuals sensors mounting offset (For now\ - expressed as a Nxn array of nD Cartesian\ - coordinates or a 1xn array where a single\ - mapping will be applied to all sensors)") + sensors = Property([PlatformSensor], doc="A list of N mounted sensors", default=[]) + mounting_offsets = Property(List[StateVector], default=None, + doc="A list of StateVectors containing the sensor translation " + "offsets from the platform's reference point. Defaults to " + "a zero vector with the same length as the Platform's mapping") + rotation_offsets = Property(List[StateVector], default=None, + doc="A list of StateVectors containing the sensor translation " + "offsets from the platform's primary axis (defined as the " + "direction of motion). Defaults to a zero vector with the " + "same length as the Platform's mapping") + mounting_mappings = Property(Union[StateVector, List[StateVector]], default=None, + doc="Mappings between the platform state vector and the" + "individual sensors mounting offset. Can be a single " + ":class:`~StateVector` (the same for all sensors) or a list " + "of :class:`~StateVector` (one for each sensor). Defaults to " + "be the same as the Platform's mapping") # TODO: Determine where a platform coordinate frame should be maintained @@ -68,46 +72,67 @@ def __init__(self, *args, **kwargs): consistent at initialisation. """ super().__init__(*args, **kwargs) - if self.mounting_mappings.max() > len(self.state.state_vector): + # Set values to defaults if not provided + if self.mounting_offsets is None: + self.mounting_offsets = [StateVector([0] * self.ndim)] * len(self.sensors) + + if self.rotation_offsets is None: + self.rotation_offsets = [StateVector([0] * 3)] * len(self.sensors) + + if self.mounting_mappings is None: + self.mounting_mappings = self.mapping + + if ((isinstance(self.mounting_mappings, Sequence) and self.mounting_mappings) + and (isinstance(self.mounting_mappings[0], np.ndarray) or + isinstance(self.mounting_mappings[0], Sequence))): + # We were passed a non-empty list of arrays or lists + self.mounting_mappings = [coerce_to_valid_mapping(m) for m in self.mounting_mappings] + elif (isinstance(self.mounting_mappings, np.ndarray) or + isinstance(self.mounting_mappings, Sequence)): + # We were passed either list of non-arrays (assumed to be ints) or a single array, so + # coerce the single entry and then expand + # noinspection PyTypeChecker + single_mapping = coerce_to_valid_mapping(self.mounting_mappings) + self.mounting_mappings = [single_mapping] * len(self.sensors) + + # Check for consistent values (after defaults have been applied) + if (self.mounting_mappings + and max(m.max() for m in self.mounting_mappings) > len(self.state_vector)): raise IndexError( "Platform state vector length and sensor mounting mapping " "are incompatible") - if len(self.sensors) != self.mounting_offsets.shape[0]: - raise IndexError( + if len(self.sensors) != len(self.mounting_offsets): + raise ValueError( "Number of sensors associated with the platform does not " "match the number of sensor mounting offsets specified") - if ((len(self.sensors) != self.mounting_mappings.shape[0]) and - (self.mounting_mappings.shape[0] != 1)): - raise IndexError( + if len(self.sensors) != len(self.rotation_offsets): + raise ValueError( + "Number of sensors associated with the platform does not " + "match the number of sensor rotation offsets specified") + + if len(self.sensors) != len(self.mounting_mappings): + raise ValueError( "Number of sensors associated with the platform does not " "match the number of mounting mappings specified") - if ((self.mounting_mappings.shape[0] == 1) and - len(self.sensors) > 1): - mapping_array = np.empty((0, self.mounting_mappings.shape[1]), int) - for i in range(len(self.sensors)): - mapping_array = np.append(mapping_array, - self.mounting_mappings, - axis=0) - self.mounting_mappings = mapping_array + # Store the platform weakref in each of the child sensors for sensor in self.sensors: sensor.platform_system = weakref.ref(self) - sensor.position = None - sensor.orientation = None - - def add_sensor(self, sensor, mounting_offset, mounting_mapping=None): - """ Determine the sensor mounting offset for the platforms relative - orientation. + def add_sensor(self, sensor: PlatformSensor, mounting_offset: StateVector = None, + rotation_offset: StateVector = None, + mounting_mapping: np.ndarray = None): + """ TODO Parameters ---------- - sensor : :class:`Sensor` + sensor : :class:`PlatformSensor` The sensor object to add - mounting_offset : :class:`np.ndarray` + mounting_offset : :class:`StateVector` A 1xN array with the mounting offset of the new sensor - mounting_mapping : :class:`np.ndarray`, optional + TODO + mounting_mapping : :class:`StateVector`, optional A 1xN array with the mounting mapping of the new sensor. If `None` (default) then use the same mapping as all previous sensors. If all sensor do not a have the same @@ -115,59 +140,43 @@ def add_sensor(self, sensor, mounting_offset, mounting_mapping=None): """ self.sensors.append(sensor) sensor.platform_system = weakref.ref(self) - sensor.position = None - sensor.orientation = None if mounting_mapping is None: - if not np.all(self.mounting_mappings - == self.mounting_mappings[0, :]): + if not all([np.all(m == self.mounting_mappings[0]) for m in self.mounting_mappings]): raise ValueError('Mapping must be specified unless all ' 'sensors have the same mapping') - mounting_mapping = self.mounting_mappings[0, :] - - if mounting_offset.ndim == 1: - mounting_offset = mounting_offset[np.newaxis, :] - if mounting_mapping.ndim == 1: - mounting_mapping = mounting_mapping[np.newaxis, :] - - if len(self.sensors) == 1: - # This is the first sensor added, so no mounting mappings/offsets - # to maintain - self.mounting_offsets = mounting_offset - else: - self.mounting_offsets = np.concatenate([self.mounting_offsets, - mounting_offset]) - - if len(self.sensors) == 1: - # This is the first sensor added, so no mounting - # mappings/offsets to maintain - self.mounting_mappings = mounting_mapping - else: - self.mounting_mappings = np.concatenate([self.mounting_mappings, - mounting_mapping]) - - def get_sensor_position(self, sensor): + if self.mounting_mappings: + mounting_mapping = self.mounting_mappings[0] + else: + # if no mapping was supplied, and no mapping is already stored, default to + # platform mapping + mounting_mapping = self.mapping + + if mounting_offset is None: + mounting_offset = StateVector([0] * self.ndim) + if rotation_offset is None: + rotation_offset = StateVector([0] * 3) + + self.mounting_mappings.append(mounting_mapping) + self.mounting_offsets.append(mounting_offset) + self.rotation_offsets.append(rotation_offset) + + def get_sensor_position(self, sensor: PlatformSensor): + # TODO docs i = self.sensors.index(sensor) if self.is_moving(): - offsets = self._get_rotated_offset(i) + offset = self._get_rotated_offset(i) else: - offsets = StateVector(self.mounting_offsets[i, :]) - new_sensor_pos = self.get_position() + offsets - return StateVector(new_sensor_pos) + offset = self.mounting_offsets[i] + new_sensor_pos = self.position + offset + return new_sensor_pos - def get_sensor_orientation(self, sensor): + def get_sensor_orientation(self, sensor: PlatformSensor): + # TODO docs + # TODO handle roll? i = self.sensors.index(sensor) - vel = np.zeros([self.mounting_mappings.shape[1], 1]) - for j in range(self.mounting_mappings.shape[1]): - vel[j, 0] = self.state.state_vector[ - self.mounting_mappings[i, j] + 1] - abs_vel, heading = cart2pol(vel[0, 0], vel[1, 0]) - return StateVector([[0], [0], [heading]]) - - def is_moving(self): - return (hasattr(self, 'transition_model') - and self.transition_model is not None - and np.any(self.get_velocity() != 0)) + offset = self.rotation_offsets[i] + return self.orientation + offset def _get_rotated_offset(self, i): """ Determine the sensor mounting offset for the platforms relative @@ -184,10 +193,18 @@ def _get_rotated_offset(self, i): Sensor mounting offset rotated relative to platform motion """ - vel = self.get_velocity() + vel = self.velocity rot = _get_rotation_matrix(vel) - return np.transpose(np.dot(rot, self.mounting_offsets[i])[np.newaxis]) + return rot @ self.mounting_offsets[i] + + +class FixedSensorPlatform(SensorPlatformMixin, FixedPlatform): + pass + + +class MovingSensorPlatform(SensorPlatformMixin, MovingPlatform): + pass def _get_rotation_matrix(vel): @@ -284,28 +301,9 @@ def _rot3d_tuple(vec): yaw = np.arctan2(vec[1], vec[0]) pitch = np.arctan2(vec[2], np.sqrt(vec[0] ** 2 + vec[1] ** 2)) * -1 - rot_z = _rot_z(yaw) + rot_z = rotz(yaw) # Modify to correct for new y axis y_axis = np.array([0, 1, 0]) rot_y = expm(np.cross(np.eye(3), np.dot(rot_z, y_axis) * pitch)) return np.dot(rot_y, rot_z) - - -def _rot_z(theta): - """ Returns a rotation matrix which will rotate a vector around the Z axis - in a counter clockwise direction by theta radians. - - Parameters - ---------- - theta : float - Required rotation angle in radians - - Returns - ------- - np.ndarray - 3x3 rotation matrix - """ - return np.array([[cos(theta), -sin(theta), 0], - [sin(theta), cos(theta), 0], - [0, 0, 1]]) diff --git a/stonesoup/platform/tests/test_platform_base.py b/stonesoup/platform/tests/test_platform_base.py index fc183f219..6bd9d6cb4 100644 --- a/stonesoup/platform/tests/test_platform_base.py +++ b/stonesoup/platform/tests/test_platform_base.py @@ -6,7 +6,7 @@ from ...types.state import State from ...models.transition.linear import ( ConstantVelocity, CombinedLinearGaussianTransitionModel) -from ..base import Platform +from ..base import MovingPlatform, FixedPlatform def test_base(): @@ -19,7 +19,7 @@ def test_base(): platform_state2d = State(np.array([[2], [2]]), timestamp) - platform = Platform(platform_state2d, None) + platform = FixedPlatform(state=platform_state2d, mapping=np.array([0, 1])) platform.move(new_timestamp) new_statevector = np.array([[2], [2]]) @@ -33,7 +33,7 @@ def test_base(): [2], [2]]), timestamp) - platform = Platform(platform_state3d, None) + platform = FixedPlatform(state=platform_state3d, mapping=[0, 1, 2]) platform.move(new_timestamp) new_statevector = np.array([[2], [2], @@ -52,7 +52,7 @@ def test_base(): [2], [1]]), timestamp) - platform = Platform(platform_state2d, model_2d) + platform = MovingPlatform(state=platform_state2d, transition_model=model_2d, mapping=[0, 2]) platform.move(new_timestamp) # Define expected platform location after movement @@ -74,7 +74,7 @@ def test_base(): [0], [1]]), timestamp) - platform = Platform(platform_state, model_3d) + platform = MovingPlatform(state=platform_state, transition_model=model_3d, mapping=[0, 2, 4]) platform.move(new_timestamp) # Define expected platform location in 3d after movement diff --git a/stonesoup/platform/tests/test_platform_simple.py b/stonesoup/platform/tests/test_platform_simple.py index 074d4a8f8..19163da01 100644 --- a/stonesoup/platform/tests/test_platform_simple.py +++ b/stonesoup/platform/tests/test_platform_simple.py @@ -5,7 +5,7 @@ import pytest from ...types.state import State -from ...platform.simple import SensorPlatform +from ...platform.simple import MovingSensorPlatform from ...models.transition.linear import ( ConstantVelocity, CombinedLinearGaussianTransitionModel) from ...sensor.radar.radar import RadarRangeBearing @@ -143,54 +143,32 @@ def radars_2d(): noise_covar = CovarianceMatrix(np.array([[0.015, 0], [0, 0.1]])) - # define arbitrary sensor origin - radar1_position = StateVector(np.array(([[100], [100]]))) - radar1_orientation = StateVector(np.array(([[0], [0], [0]]))) - radar2_position = StateVector(np.array(([[100], [100]]))) - radar2_orientation = StateVector(np.array(([[0], [0], [0]]))) - radar3_position = StateVector(np.array(([[100], [100]]))) - radar3_orientation = StateVector(np.array(([[0], [0], [0]]))) - radar4_position = StateVector(np.array(([[100], [100]]))) - radar4_orientation = StateVector(np.array(([[0], [0], [0]]))) - radar5_position = StateVector(np.array(([[100], [100]]))) - radar5_orientation = StateVector(np.array(([[0], [0], [0]]))) - measurement_mapping = np.array([0, 2]) # Create 5 simple radar sensor objects radar1 = RadarRangeBearing( - position=radar1_position, - orientation=radar1_orientation, ndim_state=4, mapping=measurement_mapping, noise_covar=noise_covar, ) radar2 = RadarRangeBearing( - position=radar2_position, - orientation=radar2_orientation, ndim_state=4, mapping=measurement_mapping, noise_covar=noise_covar ) radar3 = RadarRangeBearing( - position=radar3_position, - orientation=radar3_orientation, ndim_state=4, mapping=measurement_mapping, noise_covar=noise_covar ) radar4 = RadarRangeBearing( - position=radar4_position, - orientation=radar4_orientation, ndim_state=4, mapping=measurement_mapping, noise_covar=noise_covar ) radar5 = RadarRangeBearing( - position=radar5_position, - orientation=radar5_orientation, ndim_state=4, mapping=measurement_mapping, noise_covar=noise_covar @@ -205,72 +183,42 @@ def radars_3d(): noise_covar = CovarianceMatrix(np.array([[0.015, 0], [0, 0.1]])) - # Note 1 - the radar position is irrelevant once mounted - radar1_position = StateVector(np.array(([[100], [100], [100]]))) - radar1_orientation = StateVector(np.array(([[0], [0], [0]]))) - radar2_position = StateVector(np.array(([[100], [100], [100]]))) - radar2_orientation = StateVector(np.array(([[0], [0], [0]]))) - radar3_position = StateVector(np.array(([[100], [100], [100]]))) - radar3_orientation = StateVector(np.array(([[0], [0], [0]]))) - radar4_position = StateVector(np.array(([[100], [100], [100]]))) - radar4_orientation = StateVector(np.array(([[0], [0], [0]]))) - radar5_position = StateVector(np.array(([[100], [100], [100]]))) - radar5_orientation = StateVector(np.array(([[0], [0], [0]]))) - radar6_position = StateVector(np.array(([[100], [100], [100]]))) - radar6_orientation = StateVector(np.array(([[0], [0], [0]]))) - radar7_position = StateVector(np.array(([[100], [100], [100]]))) - radar7_orientation = StateVector(np.array(([[0], [0], [0]]))) - measurement_mapping = np.array([0, 2, 4]) # Create 5 simple radar sensor objects radar1 = RadarRangeBearing( - position=radar1_position, - orientation=radar1_orientation, ndim_state=6, mapping=measurement_mapping, noise_covar=noise_covar ) radar2 = RadarRangeBearing( - position=radar2_position, - orientation=radar2_orientation, ndim_state=6, mapping=measurement_mapping, noise_covar=noise_covar ) radar3 = RadarRangeBearing( - position=radar3_position, - orientation=radar3_orientation, ndim_state=6, mapping=measurement_mapping, noise_covar=noise_covar ) radar4 = RadarRangeBearing( - position=radar4_position, - orientation=radar4_orientation, ndim_state=6, mapping=measurement_mapping, noise_covar=noise_covar ) radar5 = RadarRangeBearing( - position=radar5_position, - orientation=radar5_orientation, ndim_state=6, mapping=measurement_mapping, noise_covar=noise_covar ) radar6 = RadarRangeBearing( - position=radar6_position, - orientation=radar6_orientation, ndim_state=6, mapping=measurement_mapping, noise_covar=noise_covar ) radar7 = RadarRangeBearing( - position=radar7_position, - orientation=radar7_orientation, ndim_state=6, mapping=measurement_mapping, noise_covar=noise_covar @@ -281,23 +229,25 @@ def radars_3d(): @pytest.fixture(scope='session') def mounting_offsets_2d(): # Generate sensor mounting offsets for testing purposes - return np.array([[0, 0], - [1, 0], - [0, 1], - [-1, 0], - [0, -1]]) + offsets = [[0, 0], + [1, 0], + [0, 1], + [-1, 0], + [0, -1]] + return [StateVector(offset) for offset in offsets] @pytest.fixture(scope='session') def mounting_offsets_3d(): # Generate sensor mounting offsets for testing purposes - return np.array([[0, 0, 0], - [1, 0, 0], - [0, 1, 0], - [-1, 0, 0], - [0, -1, 0], - [0, 0, 1], - [0, 0, -1]]) + offsets = [[0, 0, 0], + [1, 0, 0], + [0, 1, 0], + [-1, 0, 0], + [0, -1, 0], + [0, 0, 1], + [0, 0, -1]] + return [StateVector(offset) for offset in offsets] @pytest.fixture(params=[True, False], ids=["Moving", "Static"]) @@ -316,16 +266,16 @@ def mounting_mapping_on_add(request): testdata_2d = [ - np.array([[0], [0], [0], [0]]), - np.array([[10], [0], [0], [0]]), - np.array([[0], [1], [0], [0]]), - np.array([[0], [0], [0], [1]]), - np.array([[0], [-1], [0], [0]]), - np.array([[0], [0], [0], [-1]]), - np.array([[0], [1], [0], [1]]), - np.array([[0], [-1], [0], [-1]]), - np.array([[0], [1], [0], [-1]]), - np.array([[0], [-1], [0], [1]]) + StateVector([0, 0, 0, 0]), + StateVector([10, 0, 0, 0]), + StateVector([0, 1, 0, 0]), + StateVector([0, 0, 0, 1]), + StateVector([0, -1, 0, 0]), + StateVector([0, 0, 0, -1]), + StateVector([0, 1, 0, 1]), + StateVector([0, -1, 0, -1]), + StateVector([0, 1, 0, -1]), + StateVector([0, -1, 0, 1]) ] expected_2d = [ @@ -375,29 +325,31 @@ def test_2d_platform(state, expected, move, radars_2d, platform_state = State(state, timestamp) # This defines the mapping to the platforms state vector (i.e. x and y) - mounting_mappings = np.array([[0, 2]]) + mounting_mapping = np.array([0, 2]) # create a platform with the simple radar mounted if add_sensor: - platform = SensorPlatform( + platform = MovingSensorPlatform( state=platform_state, transition_model=trans_model, sensors=[], - mounting_offsets=np.array([]), - mounting_mappings=mounting_mappings + mounting_offsets=[], + mounting_mappings=mounting_mapping, + mapping=mounting_mapping ) - for i, sensor in enumerate(radars_2d): + for sensor, offset in zip(radars_2d, mounting_offsets_2d): if mounting_mapping_on_add: - platform.add_sensor(sensor, mounting_offsets_2d[i, :], - mounting_mappings) + platform.add_sensor(sensor, offset, + mounting_mapping=mounting_mapping) else: - platform.add_sensor(sensor, mounting_offsets_2d[i, :]) + platform.add_sensor(sensor, offset) else: - platform = SensorPlatform( + platform = MovingSensorPlatform( state=platform_state, transition_model=trans_model, sensors=radars_2d, mounting_offsets=mounting_offsets_2d, - mounting_mappings=mounting_mappings + mounting_mappings=mounting_mapping, + mapping=mounting_mapping ) if move: # Move the platform @@ -406,28 +358,28 @@ def test_2d_platform(state, expected, move, radars_2d, testdata_3d = [ - (np.array([[0], [0], [0], [0], [0], [0]]), get_3d_expected(0)), - (np.array([[10], [0], [0], [0], [0], [0]]), get_3d_expected(0)), - (np.array([[0], [1], [0], [0], [0], [0]]), get_3d_expected(0)), - (np.array([[0], [0], [0], [1], [0], [0]]), get_3d_expected(1)), - (np.array([[0], [-1], [0], [0], [0], [0]]), get_3d_expected(2)), - (np.array([[0], [0], [0], [-1], [0], [0]]), get_3d_expected(3)), - (np.array([[0], [1], [0], [1], [0], [0]]), get_3d_expected(4)), - (np.array([[0], [-1], [0], [-1], [0], [0]]), get_3d_expected(5)), - (np.array([[0], [1], [0], [-1], [0], [0]]), get_3d_expected(6)), - (np.array([[0], [-1], [0], [1], [0], [0]]), get_3d_expected(7)), - (np.array([[0], [0], [0], [0], [0], [1]]), get_3d_expected(8)), - (np.array([[0], [0], [0], [0], [0], [-1]]), get_3d_expected(9)), - (np.array([[0], [0], [0], [1], [0], [1]]), get_3d_expected(10)), - (np.array([[0], [0], [0], [1], [0], [-1]]), get_3d_expected(11)), - (np.array([[0], [0], [0], [-1], [0], [1]]), get_3d_expected(12)), - (np.array([[0], [0], [0], [-1], [0], [-1]]), get_3d_expected(13)), - (np.array([[0], [1], [0], [0], [0], [1]]), get_3d_expected(14)), - (np.array([[0], [-1], [0], [0], [0], [1]]), get_3d_expected(15)), - (np.array([[0], [1], [0], [0], [0], [-1]]), get_3d_expected(16)), - (np.array([[0], [-1], [0], [0], [0], [-1]]), get_3d_expected(17)), - (np.array([[0], [1], [0], [1], [0], [1]]), get_3d_expected(18)), - (np.array([[0], [-1], [0], [-1], [0], [-1]]), get_3d_expected(19)) + (StateVector([0, 0, 0, 0, 0, 0]), get_3d_expected(0)), + (StateVector([10, 0, 0, 0, 0, 0]), get_3d_expected(0)), + (StateVector([0, 1, 0, 0, 0, 0]), get_3d_expected(0)), + (StateVector([0, 0, 0, 1, 0, 0]), get_3d_expected(1)), + (StateVector([0, -1, 0, 0, 0, 0]), get_3d_expected(2)), + (StateVector([0, 0, 0, -1, 0, 0]), get_3d_expected(3)), + (StateVector([0, 1, 0, 1, 0, 0]), get_3d_expected(4)), + (StateVector([0, -1, 0, -1, 0, 0]), get_3d_expected(5)), + (StateVector([0, 1, 0, -1, 0, 0]), get_3d_expected(6)), + (StateVector([0, -1, 0, 1, 0, 0]), get_3d_expected(7)), + (StateVector([0, 0, 0, 0, 0, 1]), get_3d_expected(8)), + (StateVector([0, 0, 0, 0, 0, -1]), get_3d_expected(9)), + (StateVector([0, 0, 0, 1, 0, 1]), get_3d_expected(10)), + (StateVector([0, 0, 0, 1, 0, -1]), get_3d_expected(11)), + (StateVector([0, 0, 0, -1, 0, 1]), get_3d_expected(12)), + (StateVector([0, 0, 0, -1, 0, -1]), get_3d_expected(13)), + (StateVector([0, 1, 0, 0, 0, 1]), get_3d_expected(14)), + (StateVector([0, -1, 0, 0, 0, 1]), get_3d_expected(15)), + (StateVector([0, 1, 0, 0, 0, -1]), get_3d_expected(16)), + (StateVector([0, -1, 0, 0, 0, -1]), get_3d_expected(17)), + (StateVector([0, 1, 0, 1, 0, 1]), get_3d_expected(18)), + (StateVector([0, -1, 0, -1, 0, -1]), get_3d_expected(19)) ] @@ -447,29 +399,30 @@ def test_3d_platform(state, expected, move, radars_3d, mounting_offsets_3d): platform_state = State(state, timestamp) # This defines the mapping to the platforms state vector (i.e. x and y) - mounting_mappings = np.array([[0, 2, 4]]) + mounting_mapping = np.array([0, 2, 4]) # create a platform with the simple radar mounted if add_sensor: - platform = SensorPlatform( + platform = MovingSensorPlatform( state=platform_state, transition_model=trans_model, sensors=[], - mounting_offsets=np.array([]), - mounting_mappings=mounting_mappings + mounting_offsets=[], + mounting_mappings=mounting_mapping, + mapping=mounting_mapping ) - for i, sensor in enumerate(radars_3d): + for sensor, offset in zip(radars_3d, mounting_offsets_3d): if mounting_mapping_on_add: - platform.add_sensor(sensor, mounting_offsets_3d[i, :], - mounting_mappings) + platform.add_sensor(sensor, offset, + mounting_mapping=mounting_mapping) else: - platform.add_sensor(sensor, mounting_offsets_3d[i, :]) + platform.add_sensor(sensor, offset) else: - platform = SensorPlatform( + platform = MovingSensorPlatform( state=platform_state, transition_model=trans_model, sensors=radars_3d, mounting_offsets=mounting_offsets_3d, - mounting_mappings=mounting_mappings + mounting_mappings=mounting_mapping ) if move: # Move the platform @@ -488,14 +441,16 @@ def sensor_positions_test(expected_offset, platform): :return: """ radar_position = np.zeros( - [len(platform.sensors), platform.mounting_offsets.shape[1]]) + [len(platform.sensors), len(platform.mapping)]) expected_radar_position = np.zeros_like(radar_position) - for i in range(len(platform.sensors)): - radar_position[i, :] = platform.sensors[i].get_position().flatten() + for i, sensor in enumerate(platform.sensors): + radar_position[i, :] = sensor.position.flatten() - platform_position = platform.state.state_vector[ - platform.mounting_mappings[i, :]].flatten() + platform_position = platform.state_vector[platform.mounting_mappings[i]] expected_radar_position[i, :] = (expected_offset[i, :] + - platform_position) + platform_position.flatten()) + + assert np.allclose(radar_position[i, :], platform.get_sensor_position(sensor).flatten()) + assert np.allclose(platform_position, platform.position) assert np.allclose(expected_radar_position, radar_position) diff --git a/stonesoup/sensor/radar/radar.py b/stonesoup/sensor/radar/radar.py index 8f00bbd86..ff650cff2 100644 --- a/stonesoup/sensor/radar/radar.py +++ b/stonesoup/sensor/radar/radar.py @@ -65,7 +65,7 @@ def measure(self, ground_truth, noise=True, **kwargs): ndim_state=self.ndim_state, mapping=self.mapping, noise_covar=self.noise_covar, - translation_offset=self.get_position(), + translation_offset=self.position, rotation_offset=self.orientation) measurement_vector = measurement_model.function( @@ -442,7 +442,7 @@ def gen_probability(self, sky_state): spoiled_gain = 10 ** (self.antenna_gain / 10) * np.cos(beam_az) * np.cos(beam_el) spoiled_width = self.beam_width / (np.cos(beam_az) * np.cos(beam_el)) # state relative to radar (in cartesian space) - relative_vector = sky_state.state_vector[self.mapping] - self.get_position() + relative_vector = sky_state.state_vector[self.mapping] - self.position relative_vector = self._rotation_matrix @ relative_vector # calculate target position in spherical coordinates @@ -486,7 +486,7 @@ def measure(self, sky_state, noise=True, **kwargs): det_prob = self.gen_probability(sky_state)[0] # Is the state detected? if np.random.rand() <= det_prob: - self.measurement_model.translation_offset = self.get_position() + self.measurement_model.translation_offset = self.position self.measurement_model.rotation_offset = self.rotation_offset measured_pos = self.measurement_model.function(sky_state, noise=noise) diff --git a/stonesoup/sensor/radar/tests/test_radar.py b/stonesoup/sensor/radar/tests/test_radar.py index 84003ca80..be64625a2 100644 --- a/stonesoup/sensor/radar/tests/test_radar.py +++ b/stonesoup/sensor/radar/tests/test_radar.py @@ -13,6 +13,7 @@ from ..beam_pattern import StationaryBeam from ..beam_shape import Beam2DGaussian from ....models.measurement.linear import LinearGaussian +from ....platform.simple import FixedSensorPlatform def h2d(state_vector, translation_offset, rotation_offset): @@ -28,13 +29,13 @@ def h2d(state_vector, translation_offset, rotation_offset): [sin_z, cos_z, 0], [0, 0, 1]]) - theta_y = - rotation_offset[1, 0] + theta_y = -rotation_offset[1, 0] cos_y, sin_y = np.cos(theta_y), np.sin(theta_y) rot_y = np.array([[cos_y, 0, sin_y], [0, 1, 0], [-sin_y, 0, cos_y]]) - theta_x = - rotation_offset[0, 0] + theta_x = -rotation_offset[0, 0] cos_x, sin_x = np.cos(theta_x), np.sin(theta_x) rot_x = np.array([[1, 0, 0], [0, cos_x, -sin_x], @@ -66,14 +67,17 @@ def test_simple_radar(): timestamp=datetime.datetime.now()) measurement_mapping = np.array([0, 1]) + platform = FixedSensorPlatform(state=State(state_vector=radar_position), + orientation=radar_orientation, + mapping=[0, 1]) # Create a radar object radar = RadarRangeBearing( - position=radar_position, - orientation=radar_orientation, ndim_state=2, mapping=measurement_mapping, noise_covar=noise_covar) + platform.add_sensor(radar) + # Assert that the object has been correctly initialised assert(np.equal(radar.position, radar_position).all()) @@ -117,8 +121,6 @@ def test_rotating_radar(): # Create a radar object radar = RadarRotatingRangeBearing( - position=radar_position, - orientation=radar_orientation, ndim_state=2, mapping=measurement_mapping, noise_covar=noise_covar, @@ -127,6 +129,11 @@ def test_rotating_radar(): max_range=max_range, fov_angle=fov_angle) + platform = FixedSensorPlatform(state=State(state_vector=radar_position), + orientation=radar_orientation, + mapping=[0, 1]) + platform.add_sensor(radar) + # Assert that the object has been correctly initialised assert(np.equal(radar.position, radar_position).all()) @@ -166,8 +173,7 @@ def test_raster_scan_radar(): # The radar is facing left/east radar_orientation = StateVector([[0], [0], [np.pi]]) # The radar antenna is facing opposite the radar orientation - dwell_center = State(StateVector([[np.pi / 4]]), - timestamp=timestamp) + dwell_center = State(StateVector([[np.pi / 4]]), timestamp=timestamp) rpm = 20 # 20 Rotations Per Minute Counter-clockwise max_range = 100 # Max range of 100m fov_angle = np.pi / 12 # FOV angle of pi/12 (15 degrees) @@ -175,23 +181,23 @@ def test_raster_scan_radar(): # This will be mean the dwell center will reach at the limits -pi/2 and # pi/2. As the edge of the beam will reach the full FOV - target_state = State(radar_position + - np.array([[-5], [5]]), - timestamp=timestamp) + target_state = State(radar_position + np.array([[-5], [5]]), timestamp=timestamp) measurement_mapping = np.array([0, 1]) # Create a radar object - radar = RadarRasterScanRangeBearing( - position=radar_position, - orientation=radar_orientation, - ndim_state=2, - mapping=measurement_mapping, - noise_covar=noise_covar, - dwell_center=dwell_center, - rpm=rpm, - max_range=max_range, - fov_angle=fov_angle, - for_angle=for_angle) + radar = RadarRasterScanRangeBearing(ndim_state=2, + mapping=measurement_mapping, + noise_covar=noise_covar, + dwell_center=dwell_center, + rpm=rpm, + max_range=max_range, + fov_angle=fov_angle, + for_angle=for_angle) + + platform = FixedSensorPlatform(state=State(state_vector=radar_position), + orientation=radar_orientation, + mapping=[0, 1]) + platform.add_sensor(radar) # Assert that the object has been correctly initialised assert np.array_equal(radar.position, radar_position) @@ -234,8 +240,6 @@ def test_aesaradar(): radar = AESARadar(antenna_gain=30, mapping=[0, 2, 4], - position=StateVector([0.0] * 3), - orientation=StateVector([0.0] * 3), frequency=100e6, number_pulses=5, duty_cycle=0.1, @@ -250,6 +254,12 @@ def test_aesaradar(): centre=[np.deg2rad(15), np.deg2rad(20)]), measurement_model=None) + # noinspection PyUnusedLocal + platform = FixedSensorPlatform(state=State(state_vector=StateVector([0, 0, 0])), # noqa F841 + mapping=[0, 1, 2], + sensors=[radar], + ) + [prob_detection, snr, swer_rcs, tran_power, spoil_gain, spoil_width] = radar.gen_probability(target) assert approx(swer_rcs, 1) == 10.0 @@ -281,8 +291,12 @@ def test_swer(repeats=10000): beam_transition_model=StationaryBeam( centre=[np.deg2rad(15), np.deg2rad(20)]), measurement_model=None, - position=StateVector([0.0]*3), - orientation=StateVector([0.0]*3)) + ) + # noinspection PyUnusedLocal + platform = FixedSensorPlatform(state=State(state_vector=StateVector([0, 0, 0])), # noqa F841 + mapping=[0, 1, 2], + sensors=[radar], + ) # populate list of random rcs for i in range(0, repeats): list_rcs[i] = radar.gen_probability(target)[2] @@ -297,8 +311,6 @@ def test_swer(repeats=10000): def test_detection(): radar = AESARadar(antenna_gain=30, - position=StateVector([0.0] * 3), - orientation=StateVector([0.0] * 3), frequency=100e6, number_pulses=5, duty_cycle=0.1, @@ -315,6 +327,11 @@ def test_detection(): noise_covar=np.diag([1, 1, 1]), mapping=[0, 1, 2], ndim_state=3)) + # noinspection PyUnusedLocal + platform = FixedSensorPlatform(state=State(state_vector=StateVector([0, 0, 0])), # noqa F841 + mapping=[0, 1, 2], + sensors=[radar], + ) target = State([50e3, 10e3, 20e3], timestamp=datetime.datetime.now()) measurement = radar.measure(target) @@ -329,8 +346,6 @@ def test_failed_detect(): radar = AESARadar(antenna_gain=30, mapping=[0, 2, 4], - position=StateVector([0.0] * 3), - orientation=StateVector([0.0] * 3), frequency=100e6, number_pulses=5, duty_cycle=0.1, @@ -347,7 +362,11 @@ def test_failed_detect(): noise_covar=np.diag([1, 1, 1]), mapping=[0, 2, 4], ndim_state=6)) - + # noinspection PyUnusedLocal + platform = FixedSensorPlatform(state=State(state_vector=StateVector([0, 0, 0])), # noqa F841 + mapping=[0, 1, 2], + sensors=[radar], + ) assert radar.measure(target) is None @@ -358,26 +377,28 @@ def test_target_rcs(): rcs_20 = (GroundTruthState([250e3, 0.0, 0.0], timestamp=None)) rcs_20.rcs = 20 - radar_model = AESARadar(antenna_gain=36, - mapping=[0, 1, 2], - position=StateVector([0.0]*3), - orientation=StateVector([0.0] * 3), - frequency=10e9, - number_pulses=10, - duty_cycle=0.18, - band_width=24591.9, - beam_width=np.deg2rad(5), - rcs=None, # no default rcs - receiver_noise=5, - probability_false_alarm=5e-3, - beam_shape=Beam2DGaussian(peak_power=1e4), - measurement_model=None, - beam_transition_model=StationaryBeam(centre=[0, - 0])) - - (det_prob, snr, swer_rcs, _, _, _) = radar_model.gen_probability(rcs_10) + radar = AESARadar(antenna_gain=36, + mapping=[0, 1, 2], + frequency=10e9, + number_pulses=10, + duty_cycle=0.18, + band_width=24591.9, + beam_width=np.deg2rad(5), + rcs=None, # no default rcs + receiver_noise=5, + probability_false_alarm=5e-3, + beam_shape=Beam2DGaussian(peak_power=1e4), + measurement_model=None, + beam_transition_model=StationaryBeam(centre=[0, 0])) + # noinspection PyUnusedLocal + platform = FixedSensorPlatform(state=State(state_vector=StateVector([0, 0, 0])), # noqa F841 + mapping=[0, 1, 2], + sensors=[radar], + ) + + (det_prob, snr, swer_rcs, _, _, _) = radar.gen_probability(rcs_10) assert swer_rcs == 10 assert approx(snr, 3) == 8.197 - (det_prob, snr, swer_rcs, _, _, _) = radar_model.gen_probability(rcs_20) + (det_prob, snr, swer_rcs, _, _, _) = radar.gen_probability(rcs_20) assert swer_rcs == 20 assert round(snr, 3) == 2.125 From 523df2d508168c48535061f8c3eca0ba9aa8f7f2 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Fri, 3 Apr 2020 16:03:50 +0100 Subject: [PATCH 03/51] Simplify h2d function using exiting rot[x/y/z] functions --- stonesoup/sensor/radar/tests/test_radar.py | 20 +++----------------- 1 file changed, 3 insertions(+), 17 deletions(-) diff --git a/stonesoup/sensor/radar/tests/test_radar.py b/stonesoup/sensor/radar/tests/test_radar.py index be64625a2..8ee98aa16 100644 --- a/stonesoup/sensor/radar/tests/test_radar.py +++ b/stonesoup/sensor/radar/tests/test_radar.py @@ -3,7 +3,7 @@ from pytest import approx import numpy as np -from ....functions import cart2pol +from ....functions import cart2pol, rotz, rotx, roty from ....types.angle import Bearing from ....types.array import StateVector, CovarianceMatrix from ....types.state import State @@ -23,25 +23,11 @@ def h2d(state_vector, translation_offset, rotation_offset): [0]] # Get rotation matrix - theta_z = - rotation_offset[2, 0] - cos_z, sin_z = np.cos(theta_z), np.sin(theta_z) - rot_z = np.array([[cos_z, -sin_z, 0], - [sin_z, cos_z, 0], - [0, 0, 1]]) - + theta_z = -rotation_offset[2, 0] theta_y = -rotation_offset[1, 0] - cos_y, sin_y = np.cos(theta_y), np.sin(theta_y) - rot_y = np.array([[cos_y, 0, sin_y], - [0, 1, 0], - [-sin_y, 0, cos_y]]) - theta_x = -rotation_offset[0, 0] - cos_x, sin_x = np.cos(theta_x), np.sin(theta_x) - rot_x = np.array([[1, 0, 0], - [0, cos_x, -sin_x], - [0, sin_x, cos_x]]) - rotation_matrix = rot_z@rot_y@rot_x + rotation_matrix = rotz(theta_z)@roty(theta_y)@rotx(theta_x) xyz_rot = rotation_matrix @ xyz x = xyz_rot[0, 0] From 5e575e814b8052fdc2bc72b8d8c088112fa61689 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Fri, 3 Apr 2020 16:26:34 +0100 Subject: [PATCH 04/51] Use coerce_to_valid_mapping everywhere it is relevant --- stonesoup/sensor/passive.py | 5 +++++ stonesoup/sensor/radar/radar.py | 10 +++++++++- 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/stonesoup/sensor/passive.py b/stonesoup/sensor/passive.py index e330189fa..c17e4c4c5 100644 --- a/stonesoup/sensor/passive.py +++ b/stonesoup/sensor/passive.py @@ -1,6 +1,7 @@ # -*- coding: utf-8 -*- import numpy as np +from stonesoup.functions import coerce_to_valid_mapping from .base import Sensor3DCartesian from ..base import Property from ..models.measurement.nonlinear import CartesianToElevationBearing @@ -33,6 +34,10 @@ class PassiveElevationBearing(Sensor3DCartesian): (and follow in format) the underlying \ :class:`~.CartesianToElevationBearing` model") + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self.mapping = coerce_to_valid_mapping(self.mapping) + def measure(self, ground_truth, noise=True, **kwargs): """Generate a measurement for a given state diff --git a/stonesoup/sensor/radar/radar.py b/stonesoup/sensor/radar/radar.py index ff650cff2..afcb043d4 100644 --- a/stonesoup/sensor/radar/radar.py +++ b/stonesoup/sensor/radar/radar.py @@ -4,7 +4,7 @@ from math import erfc from ...platform.simple import PlatformSensor -from ...functions import cart2sphere, rotx, roty, rotz +from ...functions import cart2sphere, rotx, roty, rotz, coerce_to_valid_mapping from ..base import Property from ...models.measurement.nonlinear import CartesianToBearingRange @@ -43,6 +43,10 @@ class RadarRangeBearing(PlatformSensor): (and follow in format) the underlying \ :class:`~.CartesianToBearingRange` model") + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self.mapping = coerce_to_valid_mapping(self.mapping) + def measure(self, ground_truth, noise=True, **kwargs): """Generate a measurement for a given state @@ -369,6 +373,10 @@ class AESARadar(PlatformSensor): Probability, default=1e-6, doc="Probability of false alarm used in the North's approximation") + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self.mapping = coerce_to_valid_mapping(self.mapping) + @property def _snr_constant(self): temp = 290 # noise reference temperature (room temperature kelvin) From 24f5874cef9c9cbd541295a9c7205f4ee6ba6b47 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Mon, 6 Apr 2020 13:03:46 +0100 Subject: [PATCH 05/51] Remove multiple Sensor base classes and leave on the Sensor class, as all Sensors now require platforms. Passive test failing, but will be addressed in the next commit. --- stonesoup/platform/simple.py | 29 ++++------------ stonesoup/sensor/base.py | 59 ++++++++++++++++----------------- stonesoup/sensor/passive.py | 4 +-- stonesoup/sensor/radar/radar.py | 6 ++-- 4 files changed, 40 insertions(+), 58 deletions(-) diff --git a/stonesoup/platform/simple.py b/stonesoup/platform/simple.py index 04f7a6e46..463976926 100644 --- a/stonesoup/platform/simple.py +++ b/stonesoup/platform/simple.py @@ -9,30 +9,13 @@ import weakref from functools import lru_cache +from stonesoup.sensor.base import Sensor from ..functions import coerce_to_valid_mapping, rotz -from ..sensor.base import Sensor from ..base import Property from ..types.state import StateVector from .base import Platform, MovingPlatform, FixedPlatform -class PlatformSensor(Sensor): - platform_system = Property(Platform, default=None, - doc='`weakref` to the platform on which the ' - 'sensor is mounted') - - def measure(self, **kwargs): - raise NotImplementedError - - @property - def position(self): - return self.platform_system().get_sensor_position(self) - - @property - def orientation(self): - return self.platform_system().get_sensor_orientation(self) - - class SensorPlatformMixin(Platform, ABC): """A simple Platform that can carry a number of different sensors and is capable of moving based upon the :class:`~.TransitionModel`. @@ -47,7 +30,7 @@ class SensorPlatformMixin(Platform, ABC): """ - sensors = Property([PlatformSensor], doc="A list of N mounted sensors", default=[]) + sensors = Property([Sensor], doc="A list of N mounted sensors", default=[]) mounting_offsets = Property(List[StateVector], default=None, doc="A list of StateVectors containing the sensor translation " "offsets from the platform's reference point. Defaults to " @@ -121,13 +104,13 @@ def __init__(self, *args, **kwargs): for sensor in self.sensors: sensor.platform_system = weakref.ref(self) - def add_sensor(self, sensor: PlatformSensor, mounting_offset: StateVector = None, + def add_sensor(self, sensor: Sensor, mounting_offset: StateVector = None, rotation_offset: StateVector = None, mounting_mapping: np.ndarray = None): """ TODO Parameters ---------- - sensor : :class:`PlatformSensor` + sensor : :class:`Sensor` The sensor object to add mounting_offset : :class:`StateVector` A 1xN array with the mounting offset of the new sensor @@ -161,7 +144,7 @@ def add_sensor(self, sensor: PlatformSensor, mounting_offset: StateVector = None self.mounting_offsets.append(mounting_offset) self.rotation_offsets.append(rotation_offset) - def get_sensor_position(self, sensor: PlatformSensor): + def get_sensor_position(self, sensor: Sensor): # TODO docs i = self.sensors.index(sensor) if self.is_moving(): @@ -171,7 +154,7 @@ def get_sensor_position(self, sensor: PlatformSensor): new_sensor_pos = self.position + offset return new_sensor_pos - def get_sensor_orientation(self, sensor: PlatformSensor): + def get_sensor_orientation(self, sensor: Sensor): # TODO docs # TODO handle roll? i = self.sensors.index(sensor) diff --git a/stonesoup/sensor/base.py b/stonesoup/sensor/base.py index 3f4027567..f4ffd50fe 100644 --- a/stonesoup/sensor/base.py +++ b/stonesoup/sensor/base.py @@ -1,43 +1,42 @@ # -*- coding: utf-8 -*- -from abc import abstractmethod +from abc import abstractmethod, ABC + +from stonesoup.platform import Platform from ..base import Base, Property -from ..types.state import StateVector -class Sensor(Base): +class Sensor(Base, ABC): """Sensor base class - A sensor object that operates according to a given - :class:`~.MeasurementModel`. + A sensor object that operates according to a given + :class:`~.MeasurementModel`. """ + platform_system = Property(Platform, default=None, + doc='`weakref` to the platform on which the ' + 'sensor is mounted') @abstractmethod def measure(self, **kwargs): - """Generate a measurement""" raise NotImplementedError - -class Sensor3DCartesian(Sensor): - """Sensor base class extended to include 3D cartesian motion - - - """ - position = Property(StateVector, - doc="The sensor position on a 3D Cartesian plane,\ - expressed as a 3x1 array of Cartesian \ - coordinates in the order :math:`x,y,z`") - orientation = Property( - StateVector, - doc="A 3x1 array of angles (rad), specifying the sensor \ - orientation in terms of the counter-clockwise rotation \ - around each Cartesian axis in the order :math:`x,y,z`. The \ - rotation angles are positive if the rotation is in the \ - counter-clockwise direction when viewed by an observer \ - looking along the respective rotation axis, towards the \ - origin") - - @abstractmethod - def measure(self, **kwargs): - """Generate a measurement""" - raise NotImplementedError + @property + def position(self): + """The sensor position on a 3D Cartesian plane, expressed as a 3x1 array of Cartesian + coordinates in the order :math:`x,y,z` in the order :math:`x,y,z`. + + This property delegates that actual calculation of position to the platform on which the + sensor is mounted.""" + return self.platform_system().get_sensor_position(self) + + @property + def orientation(self): + """A 3x1 array of angles (rad), specifying the sensor orientation in terms of the + counter-clockwise rotation around each Cartesian axis in the order :math:`x,y,z`. + The rotation angles are positive if the rotation is in the counter-clockwise + direction when viewed by an observer looking along the respective rotation axis, + towards the origin. + + This property delegates that actual calculation of orientation to the platform on which the + sensor is mounted.""" + return self.platform_system().get_sensor_orientation(self) diff --git a/stonesoup/sensor/passive.py b/stonesoup/sensor/passive.py index c17e4c4c5..ff563e0d4 100644 --- a/stonesoup/sensor/passive.py +++ b/stonesoup/sensor/passive.py @@ -2,14 +2,14 @@ import numpy as np from stonesoup.functions import coerce_to_valid_mapping -from .base import Sensor3DCartesian +from .base import Sensor from ..base import Property from ..models.measurement.nonlinear import CartesianToElevationBearing from ..types.array import CovarianceMatrix from ..types.detection import Detection -class PassiveElevationBearing(Sensor3DCartesian): +class PassiveElevationBearing(Sensor): """A simple passive sensor that generates measurements of targets, using a :class:`~.CartesianToElevationBearing` model, relative to its position. diff --git a/stonesoup/sensor/radar/radar.py b/stonesoup/sensor/radar/radar.py index afcb043d4..6dfba6af9 100644 --- a/stonesoup/sensor/radar/radar.py +++ b/stonesoup/sensor/radar/radar.py @@ -3,7 +3,7 @@ from math import erfc -from ...platform.simple import PlatformSensor +from stonesoup.sensor.base import Sensor from ...functions import cart2sphere, rotx, roty, rotz, coerce_to_valid_mapping from ..base import Property @@ -18,7 +18,7 @@ import scipy.constants as const -class RadarRangeBearing(PlatformSensor): +class RadarRangeBearing(Sensor): """A simple radar sensor that generates measurements of targets, using a :class:`~.CartesianToBearingRange` model, relative to its position. @@ -260,7 +260,7 @@ def rotate(self, timestamp): self.rpm = -self.rpm -class AESARadar(PlatformSensor): +class AESARadar(Sensor): r"""An AESA (Active electronically scanned array) radar model that calculates the signal to noise ratio (SNR) of a target and the subsequent probability of detection (PD). The SNR is calculated using: From 143bc38ce3a8d47ce77d2e685741895de59830ac Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Mon, 6 Apr 2020 17:44:59 +0100 Subject: [PATCH 06/51] Give sensors a default fixed platform if position and or/orientation are specified --- stonesoup/platform/simple.py | 47 ++++++++++++++++++++++++++++----- stonesoup/sensor/__init__.py | 4 --- stonesoup/sensor/base.py | 13 +++++++-- stonesoup/sensor/passive.py | 2 +- stonesoup/sensor/radar/radar.py | 2 +- 5 files changed, 53 insertions(+), 15 deletions(-) diff --git a/stonesoup/platform/simple.py b/stonesoup/platform/simple.py index 463976926..e5ed7ee70 100644 --- a/stonesoup/platform/simple.py +++ b/stonesoup/platform/simple.py @@ -1,5 +1,5 @@ # -*- coding: utf-8 -*- -from abc import ABC +from abc import ABC, abstractmethod from collections.abc import Sequence from typing import List, Union @@ -9,10 +9,10 @@ import weakref from functools import lru_cache -from stonesoup.sensor.base import Sensor +from ..sensor.base import SensorBase from ..functions import coerce_to_valid_mapping, rotz from ..base import Property -from ..types.state import StateVector +from ..types.state import StateVector, State from .base import Platform, MovingPlatform, FixedPlatform @@ -30,7 +30,7 @@ class SensorPlatformMixin(Platform, ABC): """ - sensors = Property([Sensor], doc="A list of N mounted sensors", default=[]) + sensors = Property([SensorBase], doc="A list of N mounted sensors", default=[]) mounting_offsets = Property(List[StateVector], default=None, doc="A list of StateVectors containing the sensor translation " "offsets from the platform's reference point. Defaults to " @@ -104,7 +104,7 @@ def __init__(self, *args, **kwargs): for sensor in self.sensors: sensor.platform_system = weakref.ref(self) - def add_sensor(self, sensor: Sensor, mounting_offset: StateVector = None, + def add_sensor(self, sensor: SensorBase, mounting_offset: StateVector = None, rotation_offset: StateVector = None, mounting_mapping: np.ndarray = None): """ TODO @@ -144,7 +144,7 @@ def add_sensor(self, sensor: Sensor, mounting_offset: StateVector = None, self.mounting_offsets.append(mounting_offset) self.rotation_offsets.append(rotation_offset) - def get_sensor_position(self, sensor: Sensor): + def get_sensor_position(self, sensor: SensorBase): # TODO docs i = self.sensors.index(sensor) if self.is_moving(): @@ -154,7 +154,7 @@ def get_sensor_position(self, sensor: Sensor): new_sensor_pos = self.position + offset return new_sensor_pos - def get_sensor_orientation(self, sensor: Sensor): + def get_sensor_orientation(self, sensor: SensorBase): # TODO docs # TODO handle roll? i = self.sensors.index(sensor) @@ -190,6 +190,39 @@ class MovingSensorPlatform(SensorPlatformMixin, MovingPlatform): pass +class Sensor(SensorBase, ABC): + # this functionality requires knowledge of FixedSensorPlatform so cannot go in the SensorBase + # class + def __init__(self, *args, **kwargs): + position = kwargs.pop('position', None) + orientation = kwargs.pop('orientation', None) + self._internal_platform = None + super().__init__(*args, **kwargs) + if position is not None or orientation is not None: + if position is None: + # assuming 3d for a default platform + position = StateVector([0, 0, 0]) + # orientation=None will be handled correctly by the platform defaults + self._internal_platform = FixedSensorPlatform(state=State(state_vector=position), + mapping=list(range(len(position))), + orientation=orientation, + sensors=[self]) + + @property + def _has_internal_platform(self): + return self._internal_platform is not None + + def _set_platform_system(self, value): + if self._has_internal_platform: + raise AttributeError('Platform system cannot be set on sensors that were created with ' + 'a default platform') + self._property_platform_system = value + + @abstractmethod + def measure(self, **kwargs): + raise NotImplementedError + + def _get_rotation_matrix(vel): """ Generates a rotation matrix which can be used to determine the corrected sensor offsets. diff --git a/stonesoup/sensor/__init__.py b/stonesoup/sensor/__init__.py index 9b19c6eca..e69de29bb 100644 --- a/stonesoup/sensor/__init__.py +++ b/stonesoup/sensor/__init__.py @@ -1,4 +0,0 @@ -# -*- coding: utf-8 -*- -from .base import Sensor - -__all__ = ['Sensor'] diff --git a/stonesoup/sensor/base.py b/stonesoup/sensor/base.py index f4ffd50fe..4a0b9d8ee 100644 --- a/stonesoup/sensor/base.py +++ b/stonesoup/sensor/base.py @@ -1,12 +1,12 @@ # -*- coding: utf-8 -*- from abc import abstractmethod, ABC -from stonesoup.platform import Platform +from ..platform import Platform from ..base import Base, Property -class Sensor(Base, ABC): +class SensorBase(Base, ABC): """Sensor base class A sensor object that operates according to a given @@ -16,6 +16,15 @@ class Sensor(Base, ABC): doc='`weakref` to the platform on which the ' 'sensor is mounted') + # noinspection PyPropertyDefinition + @platform_system.setter + def set_platform_system(self, value): + # this slightly od construction is to allow overriding by subclasses + self._set_platform_system(value) + + def _set_platform_system(self, value): + self._property_platform_system = value + @abstractmethod def measure(self, **kwargs): raise NotImplementedError diff --git a/stonesoup/sensor/passive.py b/stonesoup/sensor/passive.py index ff563e0d4..c2092647f 100644 --- a/stonesoup/sensor/passive.py +++ b/stonesoup/sensor/passive.py @@ -2,7 +2,7 @@ import numpy as np from stonesoup.functions import coerce_to_valid_mapping -from .base import Sensor +from ..platform.simple import Sensor from ..base import Property from ..models.measurement.nonlinear import CartesianToElevationBearing from ..types.array import CovarianceMatrix diff --git a/stonesoup/sensor/radar/radar.py b/stonesoup/sensor/radar/radar.py index 6dfba6af9..79c60a7cb 100644 --- a/stonesoup/sensor/radar/radar.py +++ b/stonesoup/sensor/radar/radar.py @@ -3,7 +3,7 @@ from math import erfc -from stonesoup.sensor.base import Sensor +from stonesoup.platform.simple import Sensor from ...functions import cart2sphere, rotx, roty, rotz, coerce_to_valid_mapping from ..base import Property From 3795dec74903c49a0ae61b419c9a2ed4553d66a6 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Mon, 6 Apr 2020 19:38:41 +0100 Subject: [PATCH 07/51] Revert unnecessary platform creation in light of new default platform --- stonesoup/sensor/radar/tests/test_radar.py | 103 +++++++-------------- 1 file changed, 35 insertions(+), 68 deletions(-) diff --git a/stonesoup/sensor/radar/tests/test_radar.py b/stonesoup/sensor/radar/tests/test_radar.py index 8ee98aa16..78d112be4 100644 --- a/stonesoup/sensor/radar/tests/test_radar.py +++ b/stonesoup/sensor/radar/tests/test_radar.py @@ -13,7 +13,6 @@ from ..beam_pattern import StationaryBeam from ..beam_shape import Beam2DGaussian from ....models.measurement.linear import LinearGaussian -from ....platform.simple import FixedSensorPlatform def h2d(state_vector, translation_offset, rotation_offset): @@ -45,34 +44,26 @@ def test_simple_radar(): # Input arguments # TODO: pytest parametarization noise_covar = CovarianceMatrix([[0.015, 0], - [0, 0.1]]) + [0, 0.1]]) radar_position = StateVector([1, 1]) radar_orientation = StateVector([0, 0, 0]) - target_state = State(radar_position + - np.array([[1], [1]]), - timestamp=datetime.datetime.now()) + target_state = State(radar_position + np.array([[1], [1]]), timestamp=datetime.datetime.now()) measurement_mapping = np.array([0, 1]) - platform = FixedSensorPlatform(state=State(state_vector=radar_position), - orientation=radar_orientation, - mapping=[0, 1]) # Create a radar object - radar = RadarRangeBearing( - ndim_state=2, - mapping=measurement_mapping, - noise_covar=noise_covar) - - platform.add_sensor(radar) + radar = RadarRangeBearing(position=radar_position, + orientation=radar_orientation, + ndim_state=2, + mapping=measurement_mapping, + noise_covar=noise_covar) # Assert that the object has been correctly initialised assert(np.equal(radar.position, radar_position).all()) # Generate a noiseless measurement for the given target measurement = radar.measure(target_state, noise=False) - rho, phi = cart2pol(target_state.state_vector[0, 0] - - radar_position[0, 0], - target_state.state_vector[1, 0] - - radar_position[1, 0]) + rho, phi = cart2pol(target_state.state_vector[0, 0] - radar_position[0, 0], + target_state.state_vector[1, 0] - radar_position[1, 0]) # Assert correction of generated measurement assert(measurement.timestamp == target_state.timestamp) @@ -106,19 +97,15 @@ def test_rotating_radar(): measurement_mapping = np.array([0, 1]) # Create a radar object - radar = RadarRotatingRangeBearing( - ndim_state=2, - mapping=measurement_mapping, - noise_covar=noise_covar, - dwell_center=dwell_center, - rpm=rpm, - max_range=max_range, - fov_angle=fov_angle) - - platform = FixedSensorPlatform(state=State(state_vector=radar_position), - orientation=radar_orientation, - mapping=[0, 1]) - platform.add_sensor(radar) + radar = RadarRotatingRangeBearing(position=radar_position, + orientation=radar_orientation, + ndim_state=2, + mapping=measurement_mapping, + noise_covar=noise_covar, + dwell_center=dwell_center, + rpm=rpm, + max_range=max_range, + fov_angle=fov_angle) # Assert that the object has been correctly initialised assert(np.equal(radar.position, radar_position).all()) @@ -171,7 +158,9 @@ def test_raster_scan_radar(): measurement_mapping = np.array([0, 1]) # Create a radar object - radar = RadarRasterScanRangeBearing(ndim_state=2, + radar = RadarRasterScanRangeBearing(position=radar_position, + orientation=radar_orientation, + ndim_state=2, mapping=measurement_mapping, noise_covar=noise_covar, dwell_center=dwell_center, @@ -180,11 +169,6 @@ def test_raster_scan_radar(): fov_angle=fov_angle, for_angle=for_angle) - platform = FixedSensorPlatform(state=State(state_vector=radar_position), - orientation=radar_orientation, - mapping=[0, 1]) - platform.add_sensor(radar) - # Assert that the object has been correctly initialised assert np.array_equal(radar.position, radar_position) @@ -226,6 +210,8 @@ def test_aesaradar(): radar = AESARadar(antenna_gain=30, mapping=[0, 2, 4], + position=StateVector([0.0] * 3), + orientation=StateVector([0.0] * 3), frequency=100e6, number_pulses=5, duty_cycle=0.1, @@ -240,12 +226,6 @@ def test_aesaradar(): centre=[np.deg2rad(15), np.deg2rad(20)]), measurement_model=None) - # noinspection PyUnusedLocal - platform = FixedSensorPlatform(state=State(state_vector=StateVector([0, 0, 0])), # noqa F841 - mapping=[0, 1, 2], - sensors=[radar], - ) - [prob_detection, snr, swer_rcs, tran_power, spoil_gain, spoil_width] = radar.gen_probability(target) assert approx(swer_rcs, 1) == 10.0 @@ -277,12 +257,8 @@ def test_swer(repeats=10000): beam_transition_model=StationaryBeam( centre=[np.deg2rad(15), np.deg2rad(20)]), measurement_model=None, - ) - # noinspection PyUnusedLocal - platform = FixedSensorPlatform(state=State(state_vector=StateVector([0, 0, 0])), # noqa F841 - mapping=[0, 1, 2], - sensors=[radar], - ) + position=StateVector([0.0]*3), + orientation=StateVector([0.0]*3)) # populate list of random rcs for i in range(0, repeats): list_rcs[i] = radar.gen_probability(target)[2] @@ -297,6 +273,8 @@ def test_swer(repeats=10000): def test_detection(): radar = AESARadar(antenna_gain=30, + position=StateVector([0.0] * 3), + orientation=StateVector([0.0] * 3), frequency=100e6, number_pulses=5, duty_cycle=0.1, @@ -313,11 +291,6 @@ def test_detection(): noise_covar=np.diag([1, 1, 1]), mapping=[0, 1, 2], ndim_state=3)) - # noinspection PyUnusedLocal - platform = FixedSensorPlatform(state=State(state_vector=StateVector([0, 0, 0])), # noqa F841 - mapping=[0, 1, 2], - sensors=[radar], - ) target = State([50e3, 10e3, 20e3], timestamp=datetime.datetime.now()) measurement = radar.measure(target) @@ -332,6 +305,8 @@ def test_failed_detect(): radar = AESARadar(antenna_gain=30, mapping=[0, 2, 4], + position=StateVector([0.0] * 3), + orientation=StateVector([0.0] * 3), frequency=100e6, number_pulses=5, duty_cycle=0.1, @@ -344,15 +319,10 @@ def test_failed_detect(): beam_shape=Beam2DGaussian(peak_power=50e3), beam_transition_model=StationaryBeam( centre=[np.deg2rad(30), np.deg2rad(40)]), - measurement_model=LinearGaussian( - noise_covar=np.diag([1, 1, 1]), - mapping=[0, 2, 4], - ndim_state=6)) - # noinspection PyUnusedLocal - platform = FixedSensorPlatform(state=State(state_vector=StateVector([0, 0, 0])), # noqa F841 - mapping=[0, 1, 2], - sensors=[radar], - ) + measurement_model=LinearGaussian(noise_covar=np.diag([1, 1, 1]), + mapping=[0, 2, 4], + ndim_state=6)) + assert radar.measure(target) is None @@ -365,6 +335,8 @@ def test_target_rcs(): radar = AESARadar(antenna_gain=36, mapping=[0, 1, 2], + position=StateVector([0.0]*3), + orientation=StateVector([0.0] * 3), frequency=10e9, number_pulses=10, duty_cycle=0.18, @@ -376,11 +348,6 @@ def test_target_rcs(): beam_shape=Beam2DGaussian(peak_power=1e4), measurement_model=None, beam_transition_model=StationaryBeam(centre=[0, 0])) - # noinspection PyUnusedLocal - platform = FixedSensorPlatform(state=State(state_vector=StateVector([0, 0, 0])), # noqa F841 - mapping=[0, 1, 2], - sensors=[radar], - ) (det_prob, snr, swer_rcs, _, _, _) = radar.gen_probability(rcs_10) assert swer_rcs == 10 From edc12c2e3ebd4b8ca5126b2a8d5c8fcaf8672d13 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Tue, 7 Apr 2020 07:39:23 +0100 Subject: [PATCH 08/51] Move Sensor class to more logical location --- stonesoup/platform/simple.py | 51 ++++++--------------------------- stonesoup/sensor/base.py | 2 +- stonesoup/sensor/passive.py | 2 +- stonesoup/sensor/radar/radar.py | 2 +- stonesoup/sensor/sensor.py | 35 ++++++++++++++++++++++ 5 files changed, 47 insertions(+), 45 deletions(-) create mode 100644 stonesoup/sensor/sensor.py diff --git a/stonesoup/platform/simple.py b/stonesoup/platform/simple.py index e5ed7ee70..9734c0b19 100644 --- a/stonesoup/platform/simple.py +++ b/stonesoup/platform/simple.py @@ -1,5 +1,5 @@ # -*- coding: utf-8 -*- -from abc import ABC, abstractmethod +from abc import ABC from collections.abc import Sequence from typing import List, Union @@ -9,10 +9,10 @@ import weakref from functools import lru_cache -from ..sensor.base import SensorBase +from ..sensor.base import BaseSensor from ..functions import coerce_to_valid_mapping, rotz from ..base import Property -from ..types.state import StateVector, State +from ..types.state import StateVector from .base import Platform, MovingPlatform, FixedPlatform @@ -30,7 +30,7 @@ class SensorPlatformMixin(Platform, ABC): """ - sensors = Property([SensorBase], doc="A list of N mounted sensors", default=[]) + sensors = Property([BaseSensor], doc="A list of N mounted sensors", default=[]) mounting_offsets = Property(List[StateVector], default=None, doc="A list of StateVectors containing the sensor translation " "offsets from the platform's reference point. Defaults to " @@ -104,13 +104,13 @@ def __init__(self, *args, **kwargs): for sensor in self.sensors: sensor.platform_system = weakref.ref(self) - def add_sensor(self, sensor: SensorBase, mounting_offset: StateVector = None, + def add_sensor(self, sensor: BaseSensor, mounting_offset: StateVector = None, rotation_offset: StateVector = None, mounting_mapping: np.ndarray = None): """ TODO Parameters ---------- - sensor : :class:`Sensor` + sensor : :class:`stonesoup.sensor.sensor.Sensor` The sensor object to add mounting_offset : :class:`StateVector` A 1xN array with the mounting offset of the new sensor @@ -144,7 +144,7 @@ def add_sensor(self, sensor: SensorBase, mounting_offset: StateVector = None, self.mounting_offsets.append(mounting_offset) self.rotation_offsets.append(rotation_offset) - def get_sensor_position(self, sensor: SensorBase): + def get_sensor_position(self, sensor: BaseSensor): # TODO docs i = self.sensors.index(sensor) if self.is_moving(): @@ -154,7 +154,7 @@ def get_sensor_position(self, sensor: SensorBase): new_sensor_pos = self.position + offset return new_sensor_pos - def get_sensor_orientation(self, sensor: SensorBase): + def get_sensor_orientation(self, sensor: BaseSensor): # TODO docs # TODO handle roll? i = self.sensors.index(sensor) @@ -190,39 +190,6 @@ class MovingSensorPlatform(SensorPlatformMixin, MovingPlatform): pass -class Sensor(SensorBase, ABC): - # this functionality requires knowledge of FixedSensorPlatform so cannot go in the SensorBase - # class - def __init__(self, *args, **kwargs): - position = kwargs.pop('position', None) - orientation = kwargs.pop('orientation', None) - self._internal_platform = None - super().__init__(*args, **kwargs) - if position is not None or orientation is not None: - if position is None: - # assuming 3d for a default platform - position = StateVector([0, 0, 0]) - # orientation=None will be handled correctly by the platform defaults - self._internal_platform = FixedSensorPlatform(state=State(state_vector=position), - mapping=list(range(len(position))), - orientation=orientation, - sensors=[self]) - - @property - def _has_internal_platform(self): - return self._internal_platform is not None - - def _set_platform_system(self, value): - if self._has_internal_platform: - raise AttributeError('Platform system cannot be set on sensors that were created with ' - 'a default platform') - self._property_platform_system = value - - @abstractmethod - def measure(self, **kwargs): - raise NotImplementedError - - def _get_rotation_matrix(vel): """ Generates a rotation matrix which can be used to determine the corrected sensor offsets. @@ -299,7 +266,7 @@ def _rot3d(vec): np.ndarray 3x3 rotation matrix """ - return _rot3d_tuple(tuple(v[0] for v in vec)) + return _rot3d_tuple(tuple(vec.flat)) @lru_cache(maxsize=128) diff --git a/stonesoup/sensor/base.py b/stonesoup/sensor/base.py index 4a0b9d8ee..1c955eab5 100644 --- a/stonesoup/sensor/base.py +++ b/stonesoup/sensor/base.py @@ -6,7 +6,7 @@ from ..base import Base, Property -class SensorBase(Base, ABC): +class BaseSensor(Base, ABC): """Sensor base class A sensor object that operates according to a given diff --git a/stonesoup/sensor/passive.py b/stonesoup/sensor/passive.py index c2092647f..1ea4065bf 100644 --- a/stonesoup/sensor/passive.py +++ b/stonesoup/sensor/passive.py @@ -2,7 +2,7 @@ import numpy as np from stonesoup.functions import coerce_to_valid_mapping -from ..platform.simple import Sensor +from stonesoup.sensor.sensor import Sensor from ..base import Property from ..models.measurement.nonlinear import CartesianToElevationBearing from ..types.array import CovarianceMatrix diff --git a/stonesoup/sensor/radar/radar.py b/stonesoup/sensor/radar/radar.py index 79c60a7cb..ad036951e 100644 --- a/stonesoup/sensor/radar/radar.py +++ b/stonesoup/sensor/radar/radar.py @@ -3,7 +3,7 @@ from math import erfc -from stonesoup.platform.simple import Sensor +from stonesoup.sensor.sensor import Sensor from ...functions import cart2sphere, rotx, roty, rotz, coerce_to_valid_mapping from ..base import Property diff --git a/stonesoup/sensor/sensor.py b/stonesoup/sensor/sensor.py new file mode 100644 index 000000000..c9181ed18 --- /dev/null +++ b/stonesoup/sensor/sensor.py @@ -0,0 +1,35 @@ +from abc import ABC + +from stonesoup.platform.simple import FixedSensorPlatform +from stonesoup.sensor.base import BaseSensor +from stonesoup.types.array import StateVector +from stonesoup.types.state import State + + +class Sensor(BaseSensor, ABC): + # this functionality requires knowledge of FixedSensorPlatform so cannot go in the BaseSensor + # class + def __init__(self, *args, **kwargs): + position = kwargs.pop('position', None) + orientation = kwargs.pop('orientation', None) + self._internal_platform = None + super().__init__(*args, **kwargs) + if position is not None or orientation is not None: + if position is None: + # assuming 3d for a default platform + position = StateVector([0, 0, 0]) + # orientation=None will be handled correctly by the platform defaults + self._internal_platform = FixedSensorPlatform(state=State(state_vector=position), + mapping=list(range(len(position))), + orientation=orientation, + sensors=[self]) + + @property + def _has_internal_platform(self): + return self._internal_platform is not None + + def _set_platform_system(self, value): + if self._has_internal_platform: + raise AttributeError('Platform system cannot be set on sensors that were created with ' + 'a default platform') + self._property_platform_system = value From 0250be0f59085ae04f19a2995e8a331e67350496 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Tue, 7 Apr 2020 12:02:29 +0100 Subject: [PATCH 09/51] Fix to test_3d_platform: some permutations of test were missing --- stonesoup/platform/tests/test_platform_simple.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/stonesoup/platform/tests/test_platform_simple.py b/stonesoup/platform/tests/test_platform_simple.py index 19163da01..c82abcc47 100644 --- a/stonesoup/platform/tests/test_platform_simple.py +++ b/stonesoup/platform/tests/test_platform_simple.py @@ -389,7 +389,8 @@ def test_2d_platform(state, expected, move, radars_2d, "y.-z vel", "-y.z vel", "-y.-z vel", "x.z vel", "-x.z vel", "x.-z vel", "-x,-z vel", "x,y,z vel", "-x,-y,-z vel" ]) -def test_3d_platform(state, expected, move, radars_3d, mounting_offsets_3d): +def test_3d_platform(state, expected, move, radars_3d, mounting_offsets_3d, + add_sensor, mounting_mapping_on_add): # Define time related variables timestamp = datetime.datetime.now() # Define transition model and position for platform @@ -422,7 +423,8 @@ def test_3d_platform(state, expected, move, radars_3d, mounting_offsets_3d): transition_model=trans_model, sensors=radars_3d, mounting_offsets=mounting_offsets_3d, - mounting_mappings=mounting_mapping + mounting_mappings=mounting_mapping, + mapping=mounting_mapping ) if move: # Move the platform From 14f836a2f7062af9a2fea9512f180005c11c8ac1 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Tue, 7 Apr 2020 14:24:20 +0100 Subject: [PATCH 10/51] Convert is_moving to property --- stonesoup/platform/base.py | 2 ++ stonesoup/platform/simple.py | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/stonesoup/platform/base.py b/stonesoup/platform/base.py index 8efaab4ef..852140c13 100644 --- a/stonesoup/platform/base.py +++ b/stonesoup/platform/base.py @@ -65,6 +65,7 @@ def velocity(self): def acceleration(self): return StateVector([0] * self.ndim) + @property def is_moving(self): return False @@ -115,6 +116,7 @@ def orientation(self): _, bearing = cart2pol(*velocity) return StateVector([0, bearing]) + @property def is_moving(self): # TODO docs return (hasattr(self, 'transition_model') diff --git a/stonesoup/platform/simple.py b/stonesoup/platform/simple.py index 9734c0b19..0798f0225 100644 --- a/stonesoup/platform/simple.py +++ b/stonesoup/platform/simple.py @@ -147,7 +147,7 @@ def add_sensor(self, sensor: BaseSensor, mounting_offset: StateVector = None, def get_sensor_position(self, sensor: BaseSensor): # TODO docs i = self.sensors.index(sensor) - if self.is_moving(): + if self.is_moving: offset = self._get_rotated_offset(i) else: offset = self.mounting_offsets[i] From eff943ca06a622011ba8305152d66e271b37bd95 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Tue, 7 Apr 2020 14:25:56 +0100 Subject: [PATCH 11/51] Improve logic of is_moving, move() and bugfix in orientation of moving platform --- stonesoup/platform/base.py | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/stonesoup/platform/base.py b/stonesoup/platform/base.py index 852140c13..3b9d4804c 100644 --- a/stonesoup/platform/base.py +++ b/stonesoup/platform/base.py @@ -105,23 +105,24 @@ def acceleration(self): def orientation(self): # TODO docs # TODO handle roll? - try: - velocity = self.velocity - except AttributeError: + if not self.is_moving: raise AttributeError('Orientation of a zero-velocity moving platform is not defined') + velocity = self.velocity + if self.ndim == 3: - _, bearing, elevation = cart2sphere(*velocity) + _, bearing, elevation = cart2sphere(*velocity.flat) return StateVector([0, bearing, elevation]) elif self.ndim == 2: - _, bearing = cart2pol(*velocity) + _, bearing = cart2pol(*velocity.flat) return StateVector([0, bearing]) @property def is_moving(self): # TODO docs - return (hasattr(self, 'transition_model') - and self.transition_model is not None - and np.any(self.velocity != 0)) + # Note: a platform without a transition model can be given a velocity as part of it's + # StateVector. It just won't move + # This inconsistency will be handled in the move logic + return np.any(self.velocity != 0) def move(self, timestamp=None, **kwargs): """Propagate the platform position using the :attr:`transition_model`. @@ -147,7 +148,10 @@ def move(self, timestamp=None, **kwargs): time_interval = timestamp - self.state.timestamp except TypeError: # TypeError: (timestamp or prior.timestamp) is None - time_interval = None + return + + if self.transition_model is None: + raise AttributeError('Platform without a transition model cannot be moved') self.state = State( state_vector=self.transition_model.function( From c27d186ec65754858cc8265e4633766786f69c56 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Tue, 7 Apr 2020 14:27:34 +0100 Subject: [PATCH 12/51] Add base platform movement and orientation tests --- .../platform/tests/test_platform_base.py | 179 +++++++++++++++++- 1 file changed, 177 insertions(+), 2 deletions(-) diff --git a/stonesoup/platform/tests/test_platform_base.py b/stonesoup/platform/tests/test_platform_base.py index 6bd9d6cb4..04974d319 100644 --- a/stonesoup/platform/tests/test_platform_base.py +++ b/stonesoup/platform/tests/test_platform_base.py @@ -2,7 +2,9 @@ import datetime import numpy as np +import pytest +from stonesoup.types.array import StateVector from ...types.state import State from ...models.transition.linear import ( ConstantVelocity, CombinedLinearGaussianTransitionModel) @@ -27,6 +29,10 @@ def test_base(): assert (np.array_equal(platform.state.state_vector, new_statevector)) # Test to ensure platform time has updated assert (platform.state.timestamp == new_timestamp) + assert np.array_equal(platform.velocity, StateVector([0, 0])) + assert np.array_equal(platform.acceleration, StateVector([0, 0])) + assert platform.ndim == 2 + assert not platform.is_moving # Define a static 3d platform and check it does not move platform_state3d = State(np.array([[2], @@ -39,7 +45,11 @@ def test_base(): [2], [2]]) # Ensure 2d platform has not moved - assert (np.array_equal(platform.state.state_vector, new_statevector)) + assert np.array_equal(platform.state.state_vector, new_statevector) + assert np.array_equal(platform.velocity, StateVector([0, 0, 0])) + assert np.array_equal(platform.acceleration, StateVector([0, 0, 0])) + assert platform.ndim == 3 + assert not platform.is_moving # Define zero noise 2d constant velocity transition model model_1d = ConstantVelocity(0.0) @@ -61,6 +71,11 @@ def test_base(): [4], [1]]) assert (np.array_equal(platform.state.state_vector, new_statevector)) + assert np.array_equal(platform.velocity, StateVector([1, 1])) + with pytest.raises(AttributeError): + _ = platform.acceleration + assert platform.ndim == 2 + assert platform.is_moving # Define zero noise 3d constant velocity transition model model_3d = CombinedLinearGaussianTransitionModel( @@ -85,5 +100,165 @@ def test_base(): [2], [1]]) assert (np.array_equal(platform.state.state_vector, new_statevector)) + assert np.array_equal(platform.velocity, StateVector([1, 1, 1])) + with pytest.raises(AttributeError): + _ = platform.acceleration + assert platform.ndim == 3 + assert platform.is_moving + + +def test_velocity_properties(): + model_1d = ConstantVelocity(0.0) + model_3d = CombinedLinearGaussianTransitionModel( + [model_1d, model_1d, model_1d]) + timestamp = datetime.datetime.now() + timediff = 2 # 2sec + new_timestamp = timestamp + datetime.timedelta(seconds=timediff) + + platform_state = State(np.array([[2], + [0], + [2], + [0], + [0], + [0]]), + timestamp) + platform = MovingPlatform(state=platform_state, transition_model=model_3d, mapping=[0, 2, 4]) + old_position = platform.position + assert not platform.is_moving + assert np.array_equal(platform.velocity, StateVector([0, 0, 0])) + # check it doesn't move with timestamp = None + platform.move(timestamp=None) + assert np.array_equal(platform.position, old_position) + # check it doesn't move (as it has zero velocity) + platform.move(timestamp) + assert np.array_equal(platform.position, old_position) + platform.move(new_timestamp) + assert np.array_equal(platform.position, old_position) + + platform_state = State(np.array([[2], + [1], + [2], + [1], + [0], + [1]]), + timestamp) + platform = MovingPlatform(state=platform_state, transition_model=None, mapping=[0, 2, 4]) + assert platform.is_moving + assert np.array_equal(platform.velocity, StateVector([1, 1, 1])) + old_position = platform.position + # check it doesn't move with timestamp = None + platform.move(None) + assert np.array_equal(platform.position, old_position) + + with pytest.raises(AttributeError): + platform.move(timestamp) + + # moving platform without velocity defined + platform_state = State(np.array([[2], + [2], + [0]]), + timestamp) + platform = MovingPlatform(state=platform_state, transition_model=None, mapping=[0, 2, 4]) + with pytest.raises(AttributeError): + _ = platform.velocity + with pytest.raises(AttributeError): + _ = platform.acceleration + + +orientation_tests_3d = [(StateVector([0, 1, 0, 0, 0, 0]), StateVector([0, 0, 0])), + (StateVector([0, 0, 0, 0, 0, 1]), StateVector([0, 0, np.pi/2])), + (StateVector([0, 0, 0, 1, 0, 0]), StateVector([0, np.pi/2, 0])), + (StateVector([0, 2, 0, 0, 0, 0]), StateVector([0, 0, 0])), + (StateVector([0, 0, 0, 0, 0, 2]), StateVector([0, 0, np.pi/2])), + (StateVector([0, 0, 0, 2, 0, 0]), StateVector([0, np.pi/2, 0])), + (StateVector([0, 1, 0, 0, 0, 1]), StateVector([0, 0, np.pi/4])), + (StateVector([0, 0, 0, 1, 0, 1]), StateVector([0, np.pi/2, np.pi/4])), + (StateVector([0, 1, 0, 1, 0, 1]), + StateVector([0, np.pi/4, np.arctan(1/np.sqrt(2))])), + (StateVector([0, -1, 0, 0, 0, 0]), StateVector([0, np.pi, 0])), + (StateVector([0, 0, 0, -1, 0, 0]), StateVector([0, -np.pi/2, 0])), + (StateVector([0, 0, 0, 0, 0, -1]), StateVector([0, 0, -np.pi/2])), + (StateVector([0, -2, 0, 0, 0, 0]), StateVector([0, np.pi, 0])), + (StateVector([0, 0, 0, -2, 0, 0]), StateVector([0, -np.pi/2, 0])), + (StateVector([0, 0, 0, 0, 0, -2]), StateVector([0, 0, -np.pi/2])), + (StateVector([0, -1, 0, 0, 0, -1]), StateVector([0, np.pi, -np.pi/4])), + (StateVector([0, 0, 0, -1, 0, -1]), StateVector([0, -np.pi/2, -np.pi/4])), + ] + + +@pytest.mark.parametrize('state, orientation', orientation_tests_3d) +def test_platform_orientation_3d(state, orientation): + model_1d = ConstantVelocity(0.0) + model_3d = CombinedLinearGaussianTransitionModel( + [model_1d, model_1d, model_1d]) + timestamp = datetime.datetime.now() + timediff = 2 # 2sec + new_timestamp = timestamp + datetime.timedelta(seconds=timediff) + + platform_state = State(state, timestamp) + platform = MovingPlatform(state=platform_state, transition_model=model_3d, mapping=[0, 2, 4]) + assert np.allclose(platform.orientation, orientation) + # moving with a constant velocity model should not change the orientation + platform.move(new_timestamp) + assert np.allclose(platform.orientation, orientation) + + +orientation_tests_2d = [(StateVector([0, 1, 0, 0]), StateVector([0, 0])), + (StateVector([0, 0, 0, 1]), StateVector([0, np.pi/2])), + (StateVector([0, 2, 0, 0]), StateVector([0, 0])), + (StateVector([0, 0, 0, 2]), StateVector([0, np.pi/2])), + (StateVector([0, 1, 0, 1]), StateVector([0, np.pi/4])), + (StateVector([0, -1, 0, 0]), StateVector([0, np.pi])), + (StateVector([0, 0, 0, -1]), StateVector([0, -np.pi/2])), + (StateVector([0, -2, 0, 0]), StateVector([0, np.pi])), + (StateVector([0, 0, 0, -2]), StateVector([0, -np.pi/2])), + (StateVector([0, -1, 0, -1]), StateVector([0, -3*np.pi/4])), + ] + + +@pytest.mark.parametrize('state, orientation', orientation_tests_2d) +def test_platform_orientation_2d(state, orientation): + model_1d = ConstantVelocity(0.0) + model_2d = CombinedLinearGaussianTransitionModel( + [model_1d, model_1d]) + timestamp = datetime.datetime.now() + timediff = 2 # 2sec + new_timestamp = timestamp + datetime.timedelta(seconds=timediff) - # TODO: More assertions + platform_state = State(state, timestamp) + platform = MovingPlatform(state=platform_state, transition_model=model_2d, mapping=[0, 2]) + assert np.allclose(platform.orientation, orientation) + # moving with a constant velocity model should not change the orientation + platform.move(new_timestamp) + assert np.allclose(platform.orientation, orientation) + + +def test_orientation_error(): + # moving platform without velocity defined + timestamp = datetime.datetime.now() + platform_state = State(np.array([[2], + [2], + [0]]), + timestamp) + platform = MovingPlatform(state=platform_state, transition_model=None, mapping=[0, 1, 2]) + with pytest.raises(AttributeError): + _ = platform.orientation + platform_state = State(np.array([[2], + [0], + [2], + [0], + [2], + [0]]), + timestamp) + platform = MovingPlatform(state=platform_state, transition_model=None, mapping=[0, 2, 4]) + with pytest.raises(AttributeError): + _ = platform.orientation + + platform_state = State(np.array([[2], + [0], + [2], + [0]]), + timestamp) + platform = MovingPlatform(state=platform_state, transition_model=None, mapping=[0, 2]) + with pytest.raises(AttributeError): + _ = platform.orientation From fa41bc8ad1a5babcde7f340cb045d3931867e753 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Tue, 7 Apr 2020 14:49:15 +0100 Subject: [PATCH 13/51] Add position setting of fixed platforms with tests --- stonesoup/platform/base.py | 14 +++++ .../platform/tests/test_platform_base.py | 60 +++++++++++++++++++ 2 files changed, 74 insertions(+) diff --git a/stonesoup/platform/base.py b/stonesoup/platform/base.py index 3b9d4804c..74bc2570d 100644 --- a/stonesoup/platform/base.py +++ b/stonesoup/platform/base.py @@ -25,6 +25,10 @@ def state_vector(self): def position(self): return self.state_vector[self.mapping] + @position.setter + def position(self, value): + self._set_position(value) + @property def ndim(self): return len(self.mapping) @@ -52,11 +56,18 @@ def is_moving(self): def move(self, timestamp): raise NotImplementedError + @abstractmethod + def _set_position(self, value): + raise NotImplementedError + class FixedPlatform(Platform): orientation = Property(StateVector, default=StateVector([0, 0, 0]), doc='A fixed orientation of the static platform') + def _set_position(self, value): + self.state_vector[self.mapping] = value + @property def velocity(self): return StateVector([0] * self.ndim) @@ -124,6 +135,9 @@ def is_moving(self): # This inconsistency will be handled in the move logic return np.any(self.velocity != 0) + def _set_position(self, value): + raise AttributeError('Cannot set the position of a moving platform') + def move(self, timestamp=None, **kwargs): """Propagate the platform position using the :attr:`transition_model`. diff --git a/stonesoup/platform/tests/test_platform_base.py b/stonesoup/platform/tests/test_platform_base.py index 04974d319..f264f6143 100644 --- a/stonesoup/platform/tests/test_platform_base.py +++ b/stonesoup/platform/tests/test_platform_base.py @@ -262,3 +262,63 @@ def test_orientation_error(): platform = MovingPlatform(state=platform_state, transition_model=None, mapping=[0, 2]) with pytest.raises(AttributeError): _ = platform.orientation + + +# noinspection PyPropertyAccess +def test_setting_position(): + timestamp = datetime.datetime.now() + platform_state = State(np.array([[2], + [2], + [0]]), + timestamp) + platform = MovingPlatform(state=platform_state, transition_model=None, mapping=[0, 1, 2]) + with pytest.raises(AttributeError): + platform.position = [0, 0, 0] + with pytest.raises(AttributeError): + platform.velocity = [0, 0, 0] + with pytest.raises(AttributeError): + platform.acceleration = [0, 0, 0] + + platform_state = State(np.array([[2], + [2], + [0]]), + timestamp) + platform = FixedPlatform(state=platform_state, mapping=[0, 1, 2]) + assert np.array_equal(platform.position, StateVector([2, 2, 0])) + platform.position = StateVector([0, 0, 0]) + assert np.array_equal(platform.position, StateVector([0, 0, 0])) + assert np.array_equal(platform.state_vector, StateVector([0, 0, 0])) + + with pytest.raises(AttributeError): + platform.velocity = [0, 0, 0] + with pytest.raises(AttributeError): + platform.acceleration = [0, 0, 0] + + platform_state = State(np.array([[2], + [1], + [2], + [-1], + [2], + [0]]), + timestamp) + platform = MovingPlatform(state=platform_state, transition_model=None, mapping=[0, 1, 2]) + with pytest.raises(AttributeError): + platform.position = [0, 0] + + platform_state = State(np.array([[2], + [1], + [2], + [-1], + [2], + [0]]), + timestamp) + platform = FixedPlatform(state=platform_state, mapping=[0, 2, 4]) + assert np.array_equal(platform.position, StateVector([2, 2, 2])) + platform.position = StateVector([0, 0, 1]) + assert np.array_equal(platform.position, StateVector([0, 0, 1])) + assert np.array_equal(platform.state_vector, StateVector([0, 1, 0, -1, 1, 0])) + + with pytest.raises(AttributeError): + platform.velocity = [0, 0, 0] + with pytest.raises(AttributeError): + platform.acceleration = [0, 0, 0] From a47fa3170c8272e076cc2f262f7750789748d57b Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Tue, 7 Apr 2020 14:55:04 +0100 Subject: [PATCH 14/51] Add test of setting orientation of fixed and moving platforms --- .../platform/tests/test_platform_base.py | 48 +++++++++++++++++++ 1 file changed, 48 insertions(+) diff --git a/stonesoup/platform/tests/test_platform_base.py b/stonesoup/platform/tests/test_platform_base.py index f264f6143..d09a407b3 100644 --- a/stonesoup/platform/tests/test_platform_base.py +++ b/stonesoup/platform/tests/test_platform_base.py @@ -322,3 +322,51 @@ def test_setting_position(): platform.velocity = [0, 0, 0] with pytest.raises(AttributeError): platform.acceleration = [0, 0, 0] + + +# noinspection PyPropertyAccess +def test_setting_orientation(): + timestamp = datetime.datetime.now() + platform_state = State(np.array([[2], + [2], + [0]]), + timestamp) + platform = MovingPlatform(state=platform_state, transition_model=None, mapping=[0, 1, 2]) + with pytest.raises(AttributeError): + platform.orientation = [0, 0, 0] + + platform_state = State(np.array([[2], + [2], + [0]]), + timestamp) + platform_orientation = StateVector([0, 0, 0]) + platform = FixedPlatform(state=platform_state, mapping=[0, 1, 2], + orientation=platform_orientation) + assert np.array_equal(platform.orientation, StateVector([0, 0, 0])) + platform.orientation = StateVector([0, 1, 0]) + assert np.array_equal(platform.orientation, StateVector([0, 1, 0])) + + platform_state = State(np.array([[2], + [1], + [2], + [-1], + [2], + [0]]), + timestamp) + platform = MovingPlatform(state=platform_state, transition_model=None, mapping=[0, 1, 2]) + with pytest.raises(AttributeError): + platform.orientation = [0, 0] + + platform_state = State(np.array([[2], + [1], + [2], + [-1], + [2], + [0]]), + timestamp) + platform_orientation = StateVector([0, 0, 0]) + platform = FixedPlatform(state=platform_state, mapping=[0, 1, 2], + orientation=platform_orientation) + assert np.array_equal(platform.orientation, StateVector([0, 0, 0])) + platform.orientation = StateVector([0, 1, 0]) + assert np.array_equal(platform.orientation, StateVector([0, 1, 0])) From 9de3ba525344cc4b3e47631443f0093249a3d3a3 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Tue, 7 Apr 2020 16:05:18 +0100 Subject: [PATCH 15/51] Bugfix to off-by-one error on bounds check of mappings --- stonesoup/platform/simple.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/stonesoup/platform/simple.py b/stonesoup/platform/simple.py index 0798f0225..4bbb9e13f 100644 --- a/stonesoup/platform/simple.py +++ b/stonesoup/platform/simple.py @@ -80,7 +80,7 @@ def __init__(self, *args, **kwargs): # Check for consistent values (after defaults have been applied) if (self.mounting_mappings - and max(m.max() for m in self.mounting_mappings) > len(self.state_vector)): + and max(m.max() for m in self.mounting_mappings) >= len(self.state_vector)): raise IndexError( "Platform state vector length and sensor mounting mapping " "are incompatible") From 1b78bac3e4278a6dbc8fa5f1c53e40adcbb329a3 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Tue, 7 Apr 2020 16:06:36 +0100 Subject: [PATCH 16/51] Add tests of defaults and error handling in sensor platform --- stonesoup/functions.py | 2 +- .../platform/tests/test_platform_simple.py | 102 +++++++++++++++++- 2 files changed, 102 insertions(+), 2 deletions(-) diff --git a/stonesoup/functions.py b/stonesoup/functions.py index b3b8acee4..c62d5cd5f 100644 --- a/stonesoup/functions.py +++ b/stonesoup/functions.py @@ -570,7 +570,7 @@ def mod_elevation(x): def coerce_to_valid_mapping(mapping: Union[List[int], np.ndarray, StateVector]): - """Function to take a mapping and convert it to a suibale form for indexing a + """Function to take a mapping and convert it to a suitable form for indexing a :class:`StateVector`. Needed because if you index a :class:`StateVector` with another :class:`StateVector` (which would be the obvious technique) you get a 3d :class:`ndarray`. diff --git a/stonesoup/platform/tests/test_platform_simple.py b/stonesoup/platform/tests/test_platform_simple.py index c82abcc47..c21ab3c19 100644 --- a/stonesoup/platform/tests/test_platform_simple.py +++ b/stonesoup/platform/tests/test_platform_simple.py @@ -1,9 +1,11 @@ # coding: utf-8 +import copy import datetime import numpy as np import pytest +from stonesoup.platform.simple import FixedSensorPlatform from ...types.state import State from ...platform.simple import MovingSensorPlatform from ...models.transition.linear import ( @@ -249,6 +251,11 @@ def mounting_offsets_3d(): [0, 0, -1]] return [StateVector(offset) for offset in offsets] +@pytest.fixture(params=[MovingSensorPlatform, FixedSensorPlatform], + ids=['MovingSensorPlatform', 'FixedSensorPlatform']) +def platform_type(request): + return request.param + @pytest.fixture(params=[True, False], ids=["Moving", "Static"]) def move(request): @@ -432,6 +439,99 @@ def test_3d_platform(state, expected, move, radars_3d, mounting_offsets_3d, sensor_positions_test(expected, platform) +def test_defaults(radars_3d, platform_type, add_sensor): + platform_state = State(state_vector=StateVector([0, 1, 2, 1, 4, 1]), + timestamp=datetime.datetime.now()) + platform_args = {} + if platform_type is MovingSensorPlatform: + platform_args['transition_model'] = None + + if add_sensor: + platform = platform_type(state=platform_state, sensors=[], mapping=[0, 2, 4], + **platform_args) + for sensor in radars_3d: + platform.add_sensor(sensor) + else: + platform = platform_type(state=platform_state, sensors=radars_3d, mapping=[0, 2, 4], + **platform_args) + + for i, sensor in enumerate(radars_3d): + assert np.array_equal(platform.mounting_mappings[i], np.array([0, 2, 4])) + assert np.array_equal(platform.mounting_offsets[i], StateVector([0, 0, 0])) + assert np.array_equal(platform.rotation_offsets[i], StateVector([0, 0, 0])) + assert np.array_equal(sensor.position, platform.position) + assert np.array_equal(sensor.orientation, platform.orientation) + + +def test_add_sensor_mapping_error(radars_3d, platform_type): + platform_state = State(state_vector=StateVector([0, 1, 2, 1, 4, 1]), + timestamp=datetime.datetime.now()) + platform_args = {} + if platform_type is MovingSensorPlatform: + platform_args['transition_model'] = None + + mappings = [[0, 1, 2]] + [[0, 2, 4]] * (len(radars_3d)-1) + platform = platform_type(state=platform_state, sensors=radars_3d, mapping=[0, 2, 4], + mounting_mappings=mappings, **platform_args) + with pytest.raises(ValueError): + platform.add_sensor(radars_3d[0]) + # no error if we specify mapping + platform.add_sensor(radars_3d[0], mounting_mapping=[0, 1, 2]) + + +@pytest.mark.parametrize('mapping', [[0, 2, 4], + (0, 2, 4), + np.array([0, 2, 4])]) +def test_mounting_mapping_list(radars_3d, platform_type, mapping): + platform_state = State(state_vector=StateVector([0, 1, 2, 1, 4, 1]), + timestamp=datetime.datetime.now()) + platform_args = {} + if platform_type is MovingSensorPlatform: + platform_args['transition_model'] = None + + mappings = [mapping] * len(radars_3d) + platform = platform_type(state=platform_state, sensors=radars_3d, mapping=[0, 2, 4], + mounting_mappings=mappings, **platform_args) + + for i, sensor in enumerate(radars_3d): + assert np.array_equal(platform.mounting_mappings[i], np.array([0, 2, 4])) + + mappings = [mapping] * (len(radars_3d)-1) + with pytest.raises(ValueError): + _ = platform_type(state=platform_state, sensors=radars_3d, mapping=[0, 2, 4], + mounting_mappings=mappings, **platform_args) + + mappings = [copy.copy(mapping)] * len(radars_3d) + try: + mappings[0][2] = 6 # this value is out of bounds, and should cause an error + except TypeError: + # Tuple mapping is not assignable, but we can skip that one as the other two are good + # enough tests + return + with pytest.raises(IndexError): + _ = platform_type(state=platform_state, sensors=radars_3d, mapping=[0, 2, 4], + mounting_mappings=mappings, **platform_args) + + +def test_sensor_offset_error(radars_3d, platform_type): + platform_state = State(state_vector=StateVector([0, 1, 2, 1, 4, 1]), + timestamp=datetime.datetime.now()) + platform_args = {} + if platform_type is MovingSensorPlatform: + platform_args['transition_model'] = None + + offset = StateVector([0, 0, 0]) + + offsets = [offset] * (len(radars_3d)-1) + with pytest.raises(ValueError): + _ = platform_type(state=platform_state, sensors=radars_3d, mapping=[0, 2, 4], + mounting_offsets=offsets, **platform_args) + + with pytest.raises(ValueError): + _ = platform_type(state=platform_state, sensors=radars_3d, mapping=[0, 2, 4], + rotation_offsets=offsets, **platform_args) + + def sensor_positions_test(expected_offset, platform): """ This function asserts that the sensor positions on the platform have been @@ -446,7 +546,7 @@ def sensor_positions_test(expected_offset, platform): [len(platform.sensors), len(platform.mapping)]) expected_radar_position = np.zeros_like(radar_position) for i, sensor in enumerate(platform.sensors): - radar_position[i, :] = sensor.position.flatten() + radar_position[i, :] = sensor.position.flat platform_position = platform.state_vector[platform.mounting_mappings[i]] From 7e393774d27a66f79ff1894801262d7e2bf1e11c Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Tue, 7 Apr 2020 16:44:58 +0100 Subject: [PATCH 17/51] Add tests of base sensor functionality --- stonesoup/sensor/base.py | 2 +- stonesoup/sensor/sensor.py | 3 +- stonesoup/sensor/tests/test_sensor.py | 52 +++++++++++++++++++++++++++ 3 files changed, 55 insertions(+), 2 deletions(-) create mode 100644 stonesoup/sensor/tests/test_sensor.py diff --git a/stonesoup/sensor/base.py b/stonesoup/sensor/base.py index 1c955eab5..82d7bbde0 100644 --- a/stonesoup/sensor/base.py +++ b/stonesoup/sensor/base.py @@ -19,7 +19,7 @@ class BaseSensor(Base, ABC): # noinspection PyPropertyDefinition @platform_system.setter def set_platform_system(self, value): - # this slightly od construction is to allow overriding by subclasses + # this slightly odd construction is to allow overriding by the Sensor subclass self._set_platform_system(value) def _set_platform_system(self, value): diff --git a/stonesoup/sensor/sensor.py b/stonesoup/sensor/sensor.py index c9181ed18..3f6f2802e 100644 --- a/stonesoup/sensor/sensor.py +++ b/stonesoup/sensor/sensor.py @@ -18,7 +18,8 @@ def __init__(self, *args, **kwargs): if position is None: # assuming 3d for a default platform position = StateVector([0, 0, 0]) - # orientation=None will be handled correctly by the platform defaults + if orientation is None: + orientation = StateVector([0, 0, 0]) self._internal_platform = FixedSensorPlatform(state=State(state_vector=position), mapping=list(range(len(position))), orientation=orientation, diff --git a/stonesoup/sensor/tests/test_sensor.py b/stonesoup/sensor/tests/test_sensor.py new file mode 100644 index 000000000..03c16e838 --- /dev/null +++ b/stonesoup/sensor/tests/test_sensor.py @@ -0,0 +1,52 @@ +import datetime + +import pytest + +from stonesoup.platform.simple import FixedSensorPlatform +from stonesoup.sensor.base import BaseSensor +from stonesoup.sensor.sensor import Sensor +from stonesoup.types.array import StateVector +import numpy as np + +from stonesoup.types.state import State + + +class TSensor(Sensor): + def measure(self, **kwargs): + pass + + +class TBaseSensor(BaseSensor): + def measure(self, **kwargs): + pass + + +def test_sensor_position_setting(): + # TODO + pass + + +def test_default_platform(): + sensor = TSensor(position=StateVector([0, 0, 1])) + assert np.array_equal(sensor.position, StateVector([0, 0, 1])) + assert np.array_equal(sensor.orientation, StateVector([0, 0, 0])) + + sensor = TSensor(orientation=StateVector([0, 0, 1])) + assert np.array_equal(sensor.orientation, StateVector([0, 0, 1])) + assert np.array_equal(sensor.position, StateVector([0, 0, 0])) + + +def test_changing_platform_from_default(): + position = StateVector([0, 0, 1]) + sensor = TSensor(position=StateVector([0, 0, 1])) + + platform_state = State(state_vector=position+1, timestamp=datetime.datetime.now()) + platform = FixedSensorPlatform(state=platform_state, mapping=[0, 1, 2]) + with pytest.raises(AttributeError): + platform.add_sensor(sensor) + + +@pytest.mark.parametrize('sensor', [TBaseSensor, TSensor]) +def test_sensor_measure(sensor): + # needed for test coverage... Does no harm + assert sensor().measure() is None From 6df31b37db95c7d16e48dec71ae9127aa6094240 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Tue, 7 Apr 2020 16:58:41 +0100 Subject: [PATCH 18/51] Add position/orientation setting of sensors with default platforms and tests --- stonesoup/sensor/base.py | 24 ++++++++++++++++++++ stonesoup/sensor/tests/test_sensor.py | 32 ++++++++++++++++++++++++--- 2 files changed, 53 insertions(+), 3 deletions(-) diff --git a/stonesoup/sensor/base.py b/stonesoup/sensor/base.py index 82d7bbde0..abaa1f3ef 100644 --- a/stonesoup/sensor/base.py +++ b/stonesoup/sensor/base.py @@ -16,6 +16,10 @@ class BaseSensor(Base, ABC): doc='`weakref` to the platform on which the ' 'sensor is mounted') + @property + def platform(self): + return self.platform_system() + # noinspection PyPropertyDefinition @platform_system.setter def set_platform_system(self, value): @@ -38,6 +42,14 @@ def position(self): sensor is mounted.""" return self.platform_system().get_sensor_position(self) + @position.setter + def position(self, value): + if self._has_internal_platform: + self.platform.position = value + else: + raise AttributeError('Cannot set sensor position unless the sensor has its own ' + 'default platform') + @property def orientation(self): """A 3x1 array of angles (rad), specifying the sensor orientation in terms of the @@ -49,3 +61,15 @@ def orientation(self): This property delegates that actual calculation of orientation to the platform on which the sensor is mounted.""" return self.platform_system().get_sensor_orientation(self) + + @orientation.setter + def orientation(self, value): + if self._has_internal_platform: + self.platform.position = value + else: + raise AttributeError('Cannot set sensor position unless the sensor has its own ' + 'default platform') + + @property + def _has_internal_platform(self): + return False diff --git a/stonesoup/sensor/tests/test_sensor.py b/stonesoup/sensor/tests/test_sensor.py index 03c16e838..82a0e814f 100644 --- a/stonesoup/sensor/tests/test_sensor.py +++ b/stonesoup/sensor/tests/test_sensor.py @@ -21,9 +21,24 @@ def measure(self, **kwargs): pass -def test_sensor_position_setting(): - # TODO - pass +def test_sensor_position_orientation_setting(): + sensor = TSensor(position=StateVector([0, 0, 1])) + assert np.array_equal(sensor.position, StateVector([0, 0, 1])) + assert np.array_equal(sensor.orientation, StateVector([0, 0, 0])) + sensor.position = StateVector([0, 1, 0]) + assert np.array_equal(sensor.position, StateVector([0, 1, 0])) + sensor.orientation = StateVector([0, 1, 0]) + assert np.array_equal(sensor.orientation, StateVector([0, 0, 0])) + + position = StateVector([0, 0, 1]) + sensor = TSensor() + platform_state = State(state_vector=position + 1, timestamp=datetime.datetime.now()) + platform = FixedSensorPlatform(state=platform_state, mapping=[0, 1, 2]) + platform.add_sensor(sensor) + with pytest.raises(AttributeError): + sensor.position = StateVector([0, 1, 0]) + with pytest.raises(AttributeError): + sensor.orientation = StateVector([0, 1, 0]) def test_default_platform(): @@ -36,6 +51,17 @@ def test_default_platform(): assert np.array_equal(sensor.position, StateVector([0, 0, 0])) +def test_internal_platform_flag(): + sensor = TSensor(position=StateVector([0, 0, 1])) + assert sensor._has_internal_platform + + sensor = TSensor() + assert not sensor._has_internal_platform + + sensor = TBaseSensor() + assert not sensor._has_internal_platform + + def test_changing_platform_from_default(): position = StateVector([0, 0, 1]) sensor = TSensor(position=StateVector([0, 0, 1])) From 364c8d8ce64b1e3e9c7161471a71743a39652cde Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Wed, 8 Apr 2020 07:43:11 +0100 Subject: [PATCH 19/51] Add 3d rotation offset tests --- .../platform/tests/test_platform_simple.py | 117 +++++++++++++++++- 1 file changed, 111 insertions(+), 6 deletions(-) diff --git a/stonesoup/platform/tests/test_platform_simple.py b/stonesoup/platform/tests/test_platform_simple.py index c21ab3c19..f71a88438 100644 --- a/stonesoup/platform/tests/test_platform_simple.py +++ b/stonesoup/platform/tests/test_platform_simple.py @@ -6,6 +6,7 @@ import pytest from stonesoup.platform.simple import FixedSensorPlatform +from stonesoup.platform.tests import test_platform_base from ...types.state import State from ...platform.simple import MovingSensorPlatform from ...models.transition.linear import ( @@ -13,9 +14,6 @@ from ...sensor.radar.radar import RadarRangeBearing from ...types.array import StateVector, CovarianceMatrix -# Input arguments -# TODO: pytest parametarization - def get_3d_expected(i): if i == 0: @@ -251,6 +249,7 @@ def mounting_offsets_3d(): [0, 0, -1]] return [StateVector(offset) for offset in offsets] + @pytest.fixture(params=[MovingSensorPlatform, FixedSensorPlatform], ids=['MovingSensorPlatform', 'FixedSensorPlatform']) def platform_type(request): @@ -439,6 +438,112 @@ def test_3d_platform(state, expected, move, radars_3d, mounting_offsets_3d, sensor_positions_test(expected, platform) +@pytest.fixture(scope='session') +def rotation_offsets_2d(): + # Generate sensor mounting offsets for testing purposes + offsets = [[0, 0], + [np.pi / 4, 0], + [0, np.pi / 4], + [-np.pi / 4, 0], + [0, -np.pi / 4]] + return [StateVector(offset) for offset in offsets] + + +@pytest.fixture(scope='session') +def rotation_offsets_3d(): + # Generate sensor rotation offsets for testing purposes + offsets = [[0, 0, 0], + [np.pi / 4, 0, 0], + [0, np.pi / 4, 0], + [-np.pi / 4, 0, 0], + [0, -np.pi / 4, 0], + [0, 0, np.pi / 4], + [0, 0, -np.pi / 4]] + return [StateVector(offset) for offset in offsets] + + +def expected_orientations_3d(): + pi = np.pi + offset_3d_movement = np.arctan(1/np.sqrt(2)) + + return [ + np.array([[0., 0., 0.], [pi/4, 0., 0.], [0., pi/4, 0.], [-pi/4, 0., 0.], + [0., -pi/4, 0.], [0., 0., pi/4], [0., 0., -pi/4]]), + np.array([[0., 0., pi/2], [pi/4, 0., pi/2], [0., pi/4, pi/2], [-pi/4, 0., pi/2], + [0., -pi/4, pi/2], [0., 0., 3 * pi/4], [0., 0., pi/4]]), + np.array([[0., pi/2, 0.], + [pi/4, pi/2, 0.], [0., 3 * pi/4, 0.], [-pi/4, pi/2, 0.], + [0., pi/4, 0.], [0., pi/2, pi/4], [0., pi/2, -pi/4]]), + np.array([[0., 0., 0.], [pi/4, 0., 0.], [0., pi/4, 0.], [-pi/4, 0., 0.], + [0., -pi/4, 0.], [0., 0., pi/4], [0., 0., -pi/4]]), + np.array([[0., 0., pi/2], [pi/4, 0., pi/2], [0., pi/4, pi/2], [-pi/4, 0., pi/2], + [0., -pi/4, pi/2], [0., 0., 3 * pi/4], [0., 0., pi/4]]), + np.array([[0., pi/2, 0.], [pi/4, pi/2, 0.], [0., 3 * pi/4, 0.], [-pi/4, pi/2, 0.], + [0., pi/4, 0.], [0., pi/2, pi/4], [0., pi/2, -pi/4]]), + np.array([[0., 0., pi/4], [pi/4, 0., pi/4], [0., pi/4, pi/4], [-pi/4, 0., pi/4], + [0., -pi/4, pi/4], [0., 0., pi/2], [0., 0., 0.]]), + np.array([[0., pi/2, pi/4], [pi/4, pi/2, pi/4], [0., 3 * pi/4, pi/4], [-pi/4, pi/2, pi/4], + [0., pi/4, pi/4], [0., pi/2, pi/2], [0., pi/2, 0.]]), + np.array([[0., pi/4, offset_3d_movement], [pi/4, pi/4, offset_3d_movement], + [0., pi/2, offset_3d_movement], [-pi/4, pi/4, offset_3d_movement], + [0., 0., offset_3d_movement], [0., pi/4, pi/4 + offset_3d_movement], + [0., pi/4, -pi/4 + offset_3d_movement]]), + np.array([[0., pi, 0.], [pi/4, pi, 0.], [0., 5 * pi/4, 0.], [-pi/4, pi, 0.], + [0., 3 * pi/4, 0.], [0., pi, pi/4], [0., pi, -pi/4]]), + np.array([[0., -pi/2, 0.], [pi/4, -pi/2, 0.], [0., -pi/4, 0.], [-pi/4, -pi/2, 0.], + [0., -3 * pi/4, 0.], [0., -pi/2, pi/4], [0., -pi/2, -pi/4]]), + np.array([[0., 0., -pi/2], [pi/4, 0., -pi/2], [0., pi/4, -pi/2], [-pi/4, 0., -pi/2], + [0., -pi/4, -pi/2], [0., 0., -pi/4], [0., 0., -3 * pi/4]]), + np.array([[0., pi, 0.], [pi/4, pi, 0.], [0., 5 * pi/4, 0.], [-pi/4, pi, 0.], + [0., 3 * pi/4, 0.], [0., pi, pi/4], [0., pi, -pi/4]]), + np.array([[0., -pi/2, 0.], [pi/4, -pi/2, 0.], [0., -pi/4, 0.], [-pi/4, -pi/2, 0.], + [0., -3 * pi/4, 0.], [0., -pi/2, pi/4], [0., -pi/2, -pi/4]]), + np.array([[0., 0., -pi/2], [pi/4, 0., -pi/2], [0., pi/4, -pi/2], [-pi/4, 0., -pi/2], + [0., -pi/4, -pi/2], [0., 0., -pi/4], [0., 0., -3 * pi/4]]), + np.array([[0., pi, -pi/4], [pi/4, pi, -pi/4], [0., 5 * pi/4, -pi/4], [-pi/4, pi, -pi/4], + [0., 3 * pi/4, -pi/4], [0., pi, 0.], [0., pi, -pi/2]]), + np.array([[0., -pi/2, -pi/4], [pi/4, -pi/2, -pi/4], [0., -pi/4, -pi/4], + [-pi/4, -pi/2, -pi/4], [0., -3 * pi/4, -pi/4], [0., -pi/2, 0.], + [0., -pi/2, -pi/2]]), + ] + + +@pytest.mark.parametrize('state, expected_platform_orientation, expected_sensor_orientations', + zip(*zip(*test_platform_base.orientation_tests_3d), + expected_orientations_3d())) +def test_rotation_offsets_3d(state, expected_platform_orientation, expected_sensor_orientations, + move, radars_3d, rotation_offsets_3d): + # Define time related variables + timestamp = datetime.datetime.now() + # Define transition model and position for platform + model_1d = ConstantVelocity(0.0) # zero noise so pure movement + trans_model = CombinedLinearGaussianTransitionModel( + [model_1d] * (radars_3d[0].ndim_state // 2)) + platform_state = State(state, timestamp) + + # This defines the mapping to the platforms state vector (i.e. x and y) + mounting_mapping = np.array([0, 2, 4]) + # create a platform with the simple radar mounted + platform = MovingSensorPlatform( + state=platform_state, + transition_model=trans_model, + sensors=radars_3d, + rotation_offsets=rotation_offsets_3d, + mounting_mappings=mounting_mapping, + mapping=mounting_mapping + ) + if move: + # Move the platform + platform.move(timestamp + datetime.timedelta(seconds=2)) + assert np.allclose(platform.orientation, expected_platform_orientation) + assert np.allclose(all_sensor_orientations(platform), expected_sensor_orientations) + + +def all_sensor_orientations(platform): + radar_orientation = np.stack([sensor.orientation for sensor in platform.sensors], axis=1) + return radar_orientation.T + + def test_defaults(radars_3d, platform_type, add_sensor): platform_state = State(state_vector=StateVector([0, 1, 2, 1, 4, 1]), timestamp=datetime.datetime.now()) @@ -470,7 +575,7 @@ def test_add_sensor_mapping_error(radars_3d, platform_type): if platform_type is MovingSensorPlatform: platform_args['transition_model'] = None - mappings = [[0, 1, 2]] + [[0, 2, 4]] * (len(radars_3d)-1) + mappings = [[0, 1, 2]] + [[0, 2, 4]] * (len(radars_3d) - 1) platform = platform_type(state=platform_state, sensors=radars_3d, mapping=[0, 2, 4], mounting_mappings=mappings, **platform_args) with pytest.raises(ValueError): @@ -496,7 +601,7 @@ def test_mounting_mapping_list(radars_3d, platform_type, mapping): for i, sensor in enumerate(radars_3d): assert np.array_equal(platform.mounting_mappings[i], np.array([0, 2, 4])) - mappings = [mapping] * (len(radars_3d)-1) + mappings = [mapping] * (len(radars_3d) - 1) with pytest.raises(ValueError): _ = platform_type(state=platform_state, sensors=radars_3d, mapping=[0, 2, 4], mounting_mappings=mappings, **platform_args) @@ -522,7 +627,7 @@ def test_sensor_offset_error(radars_3d, platform_type): offset = StateVector([0, 0, 0]) - offsets = [offset] * (len(radars_3d)-1) + offsets = [offset] * (len(radars_3d) - 1) with pytest.raises(ValueError): _ = platform_type(state=platform_state, sensors=radars_3d, mapping=[0, 2, 4], mounting_offsets=offsets, **platform_args) From f5401ae8f8e39ec42b17a85fc38046ae19c07b63 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Wed, 8 Apr 2020 08:14:01 +0100 Subject: [PATCH 20/51] Add 2d rotation offset tests --- .../platform/tests/test_platform_simple.py | 56 +++++++++++++++++++ 1 file changed, 56 insertions(+) diff --git a/stonesoup/platform/tests/test_platform_simple.py b/stonesoup/platform/tests/test_platform_simple.py index f71a88438..63cbd729a 100644 --- a/stonesoup/platform/tests/test_platform_simple.py +++ b/stonesoup/platform/tests/test_platform_simple.py @@ -508,6 +508,62 @@ def expected_orientations_3d(): ] +def expected_orientations_2d(): + pi = np.pi + return [ + np.array([[0., 0.], [pi/4, 0.], [0., pi/4], [-pi/4, 0.], [0., -pi/4]]), + np.array([[0., pi/2], [pi/4, pi/2], [0., 3 * pi/4], [-pi/4, pi/2], + [0., pi/4]]), + np.array([[0., 0.], [pi/4, 0.], [0., pi/4], [-pi/4, 0.], + [0., -pi/4]]), + np.array([[0., pi/2], [pi/4, pi/2], [0., 3 * pi/4], [-pi/4, pi/2], + [0., pi/4]]), + np.array([[0., pi/4], [pi/4, pi/4], [0., pi/2], [-pi/4, pi/4], + [0., 0.]]), + np.array([[0., pi], [pi/4, pi], [0., 5*pi/4], [-pi/4, pi], + [0., 3 * pi/4]]), + np.array([[0., -pi/2], [pi/4, -pi/2], [0., -pi/4], [-pi/4, -pi/2], + [0., -3 * pi/4]]), + np.array([[0., pi], [pi/4, pi], [0., 5 * pi/4], [-pi/4, pi], + [0., 3 * pi/4]]), + np.array([[0., -pi/2], [pi/4, -pi/2], [0., -pi/4], [-pi/4, -pi/2], + [0., -3 * pi/4]]), + np.array([[0., -3 * pi/4], [pi/4, -3 * pi/4], [0., -pi/2], [-pi/4, -3 * pi/4], + [0., -pi]]) + ] + + +@pytest.mark.parametrize('state, expected_platform_orientation, expected_sensor_orientations', + zip(*zip(*test_platform_base.orientation_tests_2d), + expected_orientations_2d())) +def test_rotation_offsets_2d(state, expected_platform_orientation, expected_sensor_orientations, + move, radars_2d, rotation_offsets_2d): + # Define time related variables + timestamp = datetime.datetime.now() + # Define transition model and position for platform + model_1d = ConstantVelocity(0.0) # zero noise so pure movement + trans_model = CombinedLinearGaussianTransitionModel( + [model_1d] * (radars_2d[0].ndim_state // 2)) + platform_state = State(state, timestamp) + + # This defines the mapping to the platforms state vector (i.e. x and y) + mounting_mapping = np.array([0, 2]) + # create a platform with the simple radar mounted + platform = MovingSensorPlatform( + state=platform_state, + transition_model=trans_model, + sensors=radars_2d, + rotation_offsets=rotation_offsets_2d, + mounting_mappings=mounting_mapping, + mapping=mounting_mapping + ) + if move: + # Move the platform + platform.move(timestamp + datetime.timedelta(seconds=2)) + assert np.allclose(platform.orientation, expected_platform_orientation) + assert np.allclose(all_sensor_orientations(platform), expected_sensor_orientations) + + @pytest.mark.parametrize('state, expected_platform_orientation, expected_sensor_orientations', zip(*zip(*test_platform_base.orientation_tests_3d), expected_orientations_3d())) From de878709ca54042b4c6bee4a8cbf7ea3a6f413f0 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Wed, 8 Apr 2020 08:28:01 +0100 Subject: [PATCH 21/51] Simplify sensor_position_test code --- .../platform/tests/test_platform_simple.py | 25 ++++++++----------- 1 file changed, 11 insertions(+), 14 deletions(-) diff --git a/stonesoup/platform/tests/test_platform_simple.py b/stonesoup/platform/tests/test_platform_simple.py index 63cbd729a..4598c66f1 100644 --- a/stonesoup/platform/tests/test_platform_simple.py +++ b/stonesoup/platform/tests/test_platform_simple.py @@ -596,8 +596,14 @@ def test_rotation_offsets_3d(state, expected_platform_orientation, expected_sens def all_sensor_orientations(platform): - radar_orientation = np.stack([sensor.orientation for sensor in platform.sensors], axis=1) - return radar_orientation.T + sensor_orientations = np.concatenate([sensor.orientation for sensor in platform.sensors], + axis=1) + return sensor_orientations.T + + +def all_sensor_positions(platform): + sensor_positions = np.concatenate([sensor.position for sensor in platform.sensors], axis=1) + return sensor_positions.T def test_defaults(radars_3d, platform_type, add_sensor): @@ -703,17 +709,8 @@ def sensor_positions_test(expected_offset, platform): :param platform: platform object :return: """ - radar_position = np.zeros( - [len(platform.sensors), len(platform.mapping)]) - expected_radar_position = np.zeros_like(radar_position) - for i, sensor in enumerate(platform.sensors): - radar_position[i, :] = sensor.position.flat - - platform_position = platform.state_vector[platform.mounting_mappings[i]] - - expected_radar_position[i, :] = (expected_offset[i, :] + - platform_position.flatten()) + radar_position = all_sensor_positions(platform) + platform_position = platform.position + expected_radar_position = expected_offset + platform_position.T - assert np.allclose(radar_position[i, :], platform.get_sensor_position(sensor).flatten()) - assert np.allclose(platform_position, platform.position) assert np.allclose(expected_radar_position, radar_position) From 81923fe1d9a4b8c01436bdef31d18b106bfcf992 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Tue, 14 Apr 2020 08:13:13 +0100 Subject: [PATCH 22/51] Remove mounting_mappings and tests. They were complicating the code and unused. --- stonesoup/platform/simple.py | 60 +------------- .../platform/tests/test_platform_simple.py | 79 +------------------ 2 files changed, 7 insertions(+), 132 deletions(-) diff --git a/stonesoup/platform/simple.py b/stonesoup/platform/simple.py index 4bbb9e13f..6862ef2ea 100644 --- a/stonesoup/platform/simple.py +++ b/stonesoup/platform/simple.py @@ -1,7 +1,6 @@ # -*- coding: utf-8 -*- from abc import ABC -from collections.abc import Sequence -from typing import List, Union +from typing import List import numpy as np from math import cos, sin @@ -10,7 +9,7 @@ from functools import lru_cache from ..sensor.base import BaseSensor -from ..functions import coerce_to_valid_mapping, rotz +from ..functions import rotz from ..base import Property from ..types.state import StateVector from .base import Platform, MovingPlatform, FixedPlatform @@ -40,12 +39,6 @@ class SensorPlatformMixin(Platform, ABC): "offsets from the platform's primary axis (defined as the " "direction of motion). Defaults to a zero vector with the " "same length as the Platform's mapping") - mounting_mappings = Property(Union[StateVector, List[StateVector]], default=None, - doc="Mappings between the platform state vector and the" - "individual sensors mounting offset. Can be a single " - ":class:`~StateVector` (the same for all sensors) or a list " - "of :class:`~StateVector` (one for each sensor). Defaults to " - "be the same as the Platform's mapping") # TODO: Determine where a platform coordinate frame should be maintained @@ -62,29 +55,6 @@ def __init__(self, *args, **kwargs): if self.rotation_offsets is None: self.rotation_offsets = [StateVector([0] * 3)] * len(self.sensors) - if self.mounting_mappings is None: - self.mounting_mappings = self.mapping - - if ((isinstance(self.mounting_mappings, Sequence) and self.mounting_mappings) - and (isinstance(self.mounting_mappings[0], np.ndarray) or - isinstance(self.mounting_mappings[0], Sequence))): - # We were passed a non-empty list of arrays or lists - self.mounting_mappings = [coerce_to_valid_mapping(m) for m in self.mounting_mappings] - elif (isinstance(self.mounting_mappings, np.ndarray) or - isinstance(self.mounting_mappings, Sequence)): - # We were passed either list of non-arrays (assumed to be ints) or a single array, so - # coerce the single entry and then expand - # noinspection PyTypeChecker - single_mapping = coerce_to_valid_mapping(self.mounting_mappings) - self.mounting_mappings = [single_mapping] * len(self.sensors) - - # Check for consistent values (after defaults have been applied) - if (self.mounting_mappings - and max(m.max() for m in self.mounting_mappings) >= len(self.state_vector)): - raise IndexError( - "Platform state vector length and sensor mounting mapping " - "are incompatible") - if len(self.sensors) != len(self.mounting_offsets): raise ValueError( "Number of sensors associated with the platform does not " @@ -95,18 +65,12 @@ def __init__(self, *args, **kwargs): "Number of sensors associated with the platform does not " "match the number of sensor rotation offsets specified") - if len(self.sensors) != len(self.mounting_mappings): - raise ValueError( - "Number of sensors associated with the platform does not " - "match the number of mounting mappings specified") - # Store the platform weakref in each of the child sensors for sensor in self.sensors: sensor.platform_system = weakref.ref(self) def add_sensor(self, sensor: BaseSensor, mounting_offset: StateVector = None, - rotation_offset: StateVector = None, - mounting_mapping: np.ndarray = None): + rotation_offset: StateVector = None): """ TODO Parameters ---------- @@ -114,33 +78,15 @@ def add_sensor(self, sensor: BaseSensor, mounting_offset: StateVector = None, The sensor object to add mounting_offset : :class:`StateVector` A 1xN array with the mounting offset of the new sensor - TODO - mounting_mapping : :class:`StateVector`, optional - A 1xN array with the mounting mapping of the new sensor. - If `None` (default) then use the same mapping as all - previous sensors. If all sensor do not a have the same - mapping then raise ValueError """ self.sensors.append(sensor) sensor.platform_system = weakref.ref(self) - if mounting_mapping is None: - if not all([np.all(m == self.mounting_mappings[0]) for m in self.mounting_mappings]): - raise ValueError('Mapping must be specified unless all ' - 'sensors have the same mapping') - if self.mounting_mappings: - mounting_mapping = self.mounting_mappings[0] - else: - # if no mapping was supplied, and no mapping is already stored, default to - # platform mapping - mounting_mapping = self.mapping - if mounting_offset is None: mounting_offset = StateVector([0] * self.ndim) if rotation_offset is None: rotation_offset = StateVector([0] * 3) - self.mounting_mappings.append(mounting_mapping) self.mounting_offsets.append(mounting_offset) self.rotation_offsets.append(rotation_offset) diff --git a/stonesoup/platform/tests/test_platform_simple.py b/stonesoup/platform/tests/test_platform_simple.py index 4598c66f1..db67775a4 100644 --- a/stonesoup/platform/tests/test_platform_simple.py +++ b/stonesoup/platform/tests/test_platform_simple.py @@ -1,5 +1,4 @@ # coding: utf-8 -import copy import datetime import numpy as np @@ -266,11 +265,6 @@ def add_sensor(request): return request.param -@pytest.fixture(params=[True, False], ids=["MM_empty", "MM_added"]) -def mounting_mapping_on_add(request): - return request.param - - testdata_2d = [ StateVector([0, 0, 0, 0]), StateVector([10, 0, 0, 0]), @@ -321,7 +315,7 @@ def mounting_mapping_on_add(request): ids=["Static", "pos offset", "x vel", "y vel", "-x vel", "-y vel", "x,y vel", "-x,-y vel", "x,-y vel", "-x,y vel"]) def test_2d_platform(state, expected, move, radars_2d, - mounting_offsets_2d, add_sensor, mounting_mapping_on_add): + mounting_offsets_2d, add_sensor): # Define time related variables timestamp = datetime.datetime.now() # Define transition model and position for platform @@ -339,22 +333,16 @@ def test_2d_platform(state, expected, move, radars_2d, transition_model=trans_model, sensors=[], mounting_offsets=[], - mounting_mappings=mounting_mapping, mapping=mounting_mapping ) for sensor, offset in zip(radars_2d, mounting_offsets_2d): - if mounting_mapping_on_add: - platform.add_sensor(sensor, offset, - mounting_mapping=mounting_mapping) - else: - platform.add_sensor(sensor, offset) + platform.add_sensor(sensor, offset) else: platform = MovingSensorPlatform( state=platform_state, transition_model=trans_model, sensors=radars_2d, mounting_offsets=mounting_offsets_2d, - mounting_mappings=mounting_mapping, mapping=mounting_mapping ) if move: @@ -396,7 +384,7 @@ def test_2d_platform(state, expected, move, radars_2d, "-x,-z vel", "x,y,z vel", "-x,-y,-z vel" ]) def test_3d_platform(state, expected, move, radars_3d, mounting_offsets_3d, - add_sensor, mounting_mapping_on_add): + add_sensor): # Define time related variables timestamp = datetime.datetime.now() # Define transition model and position for platform @@ -414,22 +402,16 @@ def test_3d_platform(state, expected, move, radars_3d, mounting_offsets_3d, transition_model=trans_model, sensors=[], mounting_offsets=[], - mounting_mappings=mounting_mapping, mapping=mounting_mapping ) for sensor, offset in zip(radars_3d, mounting_offsets_3d): - if mounting_mapping_on_add: - platform.add_sensor(sensor, offset, - mounting_mapping=mounting_mapping) - else: - platform.add_sensor(sensor, offset) + platform.add_sensor(sensor, offset) else: platform = MovingSensorPlatform( state=platform_state, transition_model=trans_model, sensors=radars_3d, mounting_offsets=mounting_offsets_3d, - mounting_mappings=mounting_mapping, mapping=mounting_mapping ) if move: @@ -554,7 +536,6 @@ def test_rotation_offsets_2d(state, expected_platform_orientation, expected_sens transition_model=trans_model, sensors=radars_2d, rotation_offsets=rotation_offsets_2d, - mounting_mappings=mounting_mapping, mapping=mounting_mapping ) if move: @@ -585,7 +566,6 @@ def test_rotation_offsets_3d(state, expected_platform_orientation, expected_sens transition_model=trans_model, sensors=radars_3d, rotation_offsets=rotation_offsets_3d, - mounting_mappings=mounting_mapping, mapping=mounting_mapping ) if move: @@ -623,63 +603,12 @@ def test_defaults(radars_3d, platform_type, add_sensor): **platform_args) for i, sensor in enumerate(radars_3d): - assert np.array_equal(platform.mounting_mappings[i], np.array([0, 2, 4])) assert np.array_equal(platform.mounting_offsets[i], StateVector([0, 0, 0])) assert np.array_equal(platform.rotation_offsets[i], StateVector([0, 0, 0])) assert np.array_equal(sensor.position, platform.position) assert np.array_equal(sensor.orientation, platform.orientation) -def test_add_sensor_mapping_error(radars_3d, platform_type): - platform_state = State(state_vector=StateVector([0, 1, 2, 1, 4, 1]), - timestamp=datetime.datetime.now()) - platform_args = {} - if platform_type is MovingSensorPlatform: - platform_args['transition_model'] = None - - mappings = [[0, 1, 2]] + [[0, 2, 4]] * (len(radars_3d) - 1) - platform = platform_type(state=platform_state, sensors=radars_3d, mapping=[0, 2, 4], - mounting_mappings=mappings, **platform_args) - with pytest.raises(ValueError): - platform.add_sensor(radars_3d[0]) - # no error if we specify mapping - platform.add_sensor(radars_3d[0], mounting_mapping=[0, 1, 2]) - - -@pytest.mark.parametrize('mapping', [[0, 2, 4], - (0, 2, 4), - np.array([0, 2, 4])]) -def test_mounting_mapping_list(radars_3d, platform_type, mapping): - platform_state = State(state_vector=StateVector([0, 1, 2, 1, 4, 1]), - timestamp=datetime.datetime.now()) - platform_args = {} - if platform_type is MovingSensorPlatform: - platform_args['transition_model'] = None - - mappings = [mapping] * len(radars_3d) - platform = platform_type(state=platform_state, sensors=radars_3d, mapping=[0, 2, 4], - mounting_mappings=mappings, **platform_args) - - for i, sensor in enumerate(radars_3d): - assert np.array_equal(platform.mounting_mappings[i], np.array([0, 2, 4])) - - mappings = [mapping] * (len(radars_3d) - 1) - with pytest.raises(ValueError): - _ = platform_type(state=platform_state, sensors=radars_3d, mapping=[0, 2, 4], - mounting_mappings=mappings, **platform_args) - - mappings = [copy.copy(mapping)] * len(radars_3d) - try: - mappings[0][2] = 6 # this value is out of bounds, and should cause an error - except TypeError: - # Tuple mapping is not assignable, but we can skip that one as the other two are good - # enough tests - return - with pytest.raises(IndexError): - _ = platform_type(state=platform_state, sensors=radars_3d, mapping=[0, 2, 4], - mounting_mappings=mappings, **platform_args) - - def test_sensor_offset_error(radars_3d, platform_type): platform_state = State(state_vector=StateVector([0, 1, 2, 1, 4, 1]), timestamp=datetime.datetime.now()) From 82c548135a2372cb9b813c6847a2c7896f5fbebe Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Tue, 14 Apr 2020 09:42:53 +0100 Subject: [PATCH 23/51] Rename Platform.mapping to position_mapping and add velocity_mapping --- stonesoup/platform/base.py | 17 +++--- stonesoup/platform/simple.py | 5 +- .../platform/tests/test_platform_base.py | 53 ++++++++++++------- .../platform/tests/test_platform_simple.py | 30 +++++------ stonesoup/sensor/sensor.py | 9 ++-- stonesoup/sensor/tests/test_sensor.py | 26 ++++----- 6 files changed, 80 insertions(+), 60 deletions(-) diff --git a/stonesoup/platform/base.py b/stonesoup/platform/base.py index 74bc2570d..b35b58775 100644 --- a/stonesoup/platform/base.py +++ b/stonesoup/platform/base.py @@ -11,11 +11,16 @@ class Platform(Base, ABC): state = Property(State, doc="The platform state at any given point") - mapping = Property(np.ndarray, doc="Mapping between platform position and state dims") + position_mapping = Property(np.ndarray, doc="Mapping between platform position and state dims") + velocity_mapping = Property(np.ndarray, default=None, + doc="Mapping between platform velocity and state dims. If not " + "set, it will default to `[m+1 for m in position_mapping]`") def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) - self.mapping = coerce_to_valid_mapping(self.mapping) + self.position_mapping = coerce_to_valid_mapping(self.position_mapping) + if self.velocity_mapping is None: + self.velocity_mapping = self.position_mapping + 1 @property def state_vector(self): @@ -23,7 +28,7 @@ def state_vector(self): @property def position(self): - return self.state_vector[self.mapping] + return self.state_vector[self.position_mapping] @position.setter def position(self, value): @@ -31,7 +36,7 @@ def position(self, value): @property def ndim(self): - return len(self.mapping) + return len(self.position_mapping) @property @abstractmethod @@ -66,7 +71,7 @@ class FixedPlatform(Platform): doc='A fixed orientation of the static platform') def _set_position(self, value): - self.state_vector[self.mapping] = value + self.state_vector[self.position_mapping] = value @property def velocity(self): @@ -99,7 +104,7 @@ def velocity(self): # TODO docs # TODO return zeros? try: - return self.state_vector[self.mapping + 1] + return self.state_vector[self.velocity_mapping] except IndexError: raise AttributeError('Velocity is not defined for this platform') diff --git a/stonesoup/platform/simple.py b/stonesoup/platform/simple.py index 6862ef2ea..a6dfa2c9c 100644 --- a/stonesoup/platform/simple.py +++ b/stonesoup/platform/simple.py @@ -33,12 +33,13 @@ class SensorPlatformMixin(Platform, ABC): mounting_offsets = Property(List[StateVector], default=None, doc="A list of StateVectors containing the sensor translation " "offsets from the platform's reference point. Defaults to " - "a zero vector with the same length as the Platform's mapping") + "a zero vector with the same length as the Platform's " + "position_mapping") rotation_offsets = Property(List[StateVector], default=None, doc="A list of StateVectors containing the sensor translation " "offsets from the platform's primary axis (defined as the " "direction of motion). Defaults to a zero vector with the " - "same length as the Platform's mapping") + "same length as the Platform's position_mapping") # TODO: Determine where a platform coordinate frame should be maintained diff --git a/stonesoup/platform/tests/test_platform_base.py b/stonesoup/platform/tests/test_platform_base.py index d09a407b3..04419e097 100644 --- a/stonesoup/platform/tests/test_platform_base.py +++ b/stonesoup/platform/tests/test_platform_base.py @@ -21,7 +21,7 @@ def test_base(): platform_state2d = State(np.array([[2], [2]]), timestamp) - platform = FixedPlatform(state=platform_state2d, mapping=np.array([0, 1])) + platform = FixedPlatform(state=platform_state2d, position_mapping=np.array([0, 1])) platform.move(new_timestamp) new_statevector = np.array([[2], [2]]) @@ -39,7 +39,7 @@ def test_base(): [2], [2]]), timestamp) - platform = FixedPlatform(state=platform_state3d, mapping=[0, 1, 2]) + platform = FixedPlatform(state=platform_state3d, position_mapping=[0, 1, 2]) platform.move(new_timestamp) new_statevector = np.array([[2], [2], @@ -62,7 +62,8 @@ def test_base(): [2], [1]]), timestamp) - platform = MovingPlatform(state=platform_state2d, transition_model=model_2d, mapping=[0, 2]) + platform = MovingPlatform(state=platform_state2d, transition_model=model_2d, + position_mapping=[0, 2]) platform.move(new_timestamp) # Define expected platform location after movement @@ -89,7 +90,8 @@ def test_base(): [0], [1]]), timestamp) - platform = MovingPlatform(state=platform_state, transition_model=model_3d, mapping=[0, 2, 4]) + platform = MovingPlatform(state=platform_state, transition_model=model_3d, + position_mapping=[0, 2, 4]) platform.move(new_timestamp) # Define expected platform location in 3d after movement @@ -122,7 +124,8 @@ def test_velocity_properties(): [0], [0]]), timestamp) - platform = MovingPlatform(state=platform_state, transition_model=model_3d, mapping=[0, 2, 4]) + platform = MovingPlatform(state=platform_state, transition_model=model_3d, + position_mapping=[0, 2, 4]) old_position = platform.position assert not platform.is_moving assert np.array_equal(platform.velocity, StateVector([0, 0, 0])) @@ -142,7 +145,8 @@ def test_velocity_properties(): [0], [1]]), timestamp) - platform = MovingPlatform(state=platform_state, transition_model=None, mapping=[0, 2, 4]) + platform = MovingPlatform(state=platform_state, transition_model=None, + position_mapping=[0, 2, 4]) assert platform.is_moving assert np.array_equal(platform.velocity, StateVector([1, 1, 1])) old_position = platform.position @@ -158,7 +162,8 @@ def test_velocity_properties(): [2], [0]]), timestamp) - platform = MovingPlatform(state=platform_state, transition_model=None, mapping=[0, 2, 4]) + platform = MovingPlatform(state=platform_state, transition_model=None, + position_mapping=[0, 2, 4]) with pytest.raises(AttributeError): _ = platform.velocity with pytest.raises(AttributeError): @@ -196,7 +201,8 @@ def test_platform_orientation_3d(state, orientation): new_timestamp = timestamp + datetime.timedelta(seconds=timediff) platform_state = State(state, timestamp) - platform = MovingPlatform(state=platform_state, transition_model=model_3d, mapping=[0, 2, 4]) + platform = MovingPlatform(state=platform_state, transition_model=model_3d, + position_mapping=[0, 2, 4]) assert np.allclose(platform.orientation, orientation) # moving with a constant velocity model should not change the orientation platform.move(new_timestamp) @@ -226,7 +232,8 @@ def test_platform_orientation_2d(state, orientation): new_timestamp = timestamp + datetime.timedelta(seconds=timediff) platform_state = State(state, timestamp) - platform = MovingPlatform(state=platform_state, transition_model=model_2d, mapping=[0, 2]) + platform = MovingPlatform(state=platform_state, transition_model=model_2d, + position_mapping=[0, 2]) assert np.allclose(platform.orientation, orientation) # moving with a constant velocity model should not change the orientation platform.move(new_timestamp) @@ -240,7 +247,8 @@ def test_orientation_error(): [2], [0]]), timestamp) - platform = MovingPlatform(state=platform_state, transition_model=None, mapping=[0, 1, 2]) + platform = MovingPlatform(state=platform_state, transition_model=None, + position_mapping=[0, 1, 2]) with pytest.raises(AttributeError): _ = platform.orientation platform_state = State(np.array([[2], @@ -250,7 +258,8 @@ def test_orientation_error(): [2], [0]]), timestamp) - platform = MovingPlatform(state=platform_state, transition_model=None, mapping=[0, 2, 4]) + platform = MovingPlatform(state=platform_state, transition_model=None, + position_mapping=[0, 2, 4]) with pytest.raises(AttributeError): _ = platform.orientation @@ -259,7 +268,7 @@ def test_orientation_error(): [2], [0]]), timestamp) - platform = MovingPlatform(state=platform_state, transition_model=None, mapping=[0, 2]) + platform = MovingPlatform(state=platform_state, transition_model=None, position_mapping=[0, 2]) with pytest.raises(AttributeError): _ = platform.orientation @@ -271,7 +280,8 @@ def test_setting_position(): [2], [0]]), timestamp) - platform = MovingPlatform(state=platform_state, transition_model=None, mapping=[0, 1, 2]) + platform = MovingPlatform(state=platform_state, transition_model=None, + position_mapping=[0, 1, 2]) with pytest.raises(AttributeError): platform.position = [0, 0, 0] with pytest.raises(AttributeError): @@ -283,7 +293,7 @@ def test_setting_position(): [2], [0]]), timestamp) - platform = FixedPlatform(state=platform_state, mapping=[0, 1, 2]) + platform = FixedPlatform(state=platform_state, position_mapping=[0, 1, 2]) assert np.array_equal(platform.position, StateVector([2, 2, 0])) platform.position = StateVector([0, 0, 0]) assert np.array_equal(platform.position, StateVector([0, 0, 0])) @@ -301,7 +311,8 @@ def test_setting_position(): [2], [0]]), timestamp) - platform = MovingPlatform(state=platform_state, transition_model=None, mapping=[0, 1, 2]) + platform = MovingPlatform(state=platform_state, transition_model=None, + position_mapping=[0, 1, 2]) with pytest.raises(AttributeError): platform.position = [0, 0] @@ -312,7 +323,7 @@ def test_setting_position(): [2], [0]]), timestamp) - platform = FixedPlatform(state=platform_state, mapping=[0, 2, 4]) + platform = FixedPlatform(state=platform_state, position_mapping=[0, 2, 4]) assert np.array_equal(platform.position, StateVector([2, 2, 2])) platform.position = StateVector([0, 0, 1]) assert np.array_equal(platform.position, StateVector([0, 0, 1])) @@ -331,7 +342,8 @@ def test_setting_orientation(): [2], [0]]), timestamp) - platform = MovingPlatform(state=platform_state, transition_model=None, mapping=[0, 1, 2]) + platform = MovingPlatform(state=platform_state, transition_model=None, + position_mapping=[0, 1, 2]) with pytest.raises(AttributeError): platform.orientation = [0, 0, 0] @@ -340,7 +352,7 @@ def test_setting_orientation(): [0]]), timestamp) platform_orientation = StateVector([0, 0, 0]) - platform = FixedPlatform(state=platform_state, mapping=[0, 1, 2], + platform = FixedPlatform(state=platform_state, position_mapping=[0, 1, 2], orientation=platform_orientation) assert np.array_equal(platform.orientation, StateVector([0, 0, 0])) platform.orientation = StateVector([0, 1, 0]) @@ -353,7 +365,8 @@ def test_setting_orientation(): [2], [0]]), timestamp) - platform = MovingPlatform(state=platform_state, transition_model=None, mapping=[0, 1, 2]) + platform = MovingPlatform(state=platform_state, transition_model=None, + position_mapping=[0, 1, 2]) with pytest.raises(AttributeError): platform.orientation = [0, 0] @@ -365,7 +378,7 @@ def test_setting_orientation(): [0]]), timestamp) platform_orientation = StateVector([0, 0, 0]) - platform = FixedPlatform(state=platform_state, mapping=[0, 1, 2], + platform = FixedPlatform(state=platform_state, position_mapping=[0, 1, 2], orientation=platform_orientation) assert np.array_equal(platform.orientation, StateVector([0, 0, 0])) platform.orientation = StateVector([0, 1, 0]) diff --git a/stonesoup/platform/tests/test_platform_simple.py b/stonesoup/platform/tests/test_platform_simple.py index db67775a4..9e4bd0dc8 100644 --- a/stonesoup/platform/tests/test_platform_simple.py +++ b/stonesoup/platform/tests/test_platform_simple.py @@ -324,7 +324,7 @@ def test_2d_platform(state, expected, move, radars_2d, [model_1d] * (radars_2d[0].ndim_state // 2)) platform_state = State(state, timestamp) - # This defines the mapping to the platforms state vector (i.e. x and y) + # This defines the position_mapping to the platforms state vector (i.e. x and y) mounting_mapping = np.array([0, 2]) # create a platform with the simple radar mounted if add_sensor: @@ -333,7 +333,7 @@ def test_2d_platform(state, expected, move, radars_2d, transition_model=trans_model, sensors=[], mounting_offsets=[], - mapping=mounting_mapping + position_mapping=mounting_mapping ) for sensor, offset in zip(radars_2d, mounting_offsets_2d): platform.add_sensor(sensor, offset) @@ -343,7 +343,7 @@ def test_2d_platform(state, expected, move, radars_2d, transition_model=trans_model, sensors=radars_2d, mounting_offsets=mounting_offsets_2d, - mapping=mounting_mapping + position_mapping=mounting_mapping ) if move: # Move the platform @@ -393,7 +393,7 @@ def test_3d_platform(state, expected, move, radars_3d, mounting_offsets_3d, [model_1d] * (radars_3d[0].ndim_state // 2)) platform_state = State(state, timestamp) - # This defines the mapping to the platforms state vector (i.e. x and y) + # This defines the position_mapping to the platforms state vector (i.e. x and y) mounting_mapping = np.array([0, 2, 4]) # create a platform with the simple radar mounted if add_sensor: @@ -402,7 +402,7 @@ def test_3d_platform(state, expected, move, radars_3d, mounting_offsets_3d, transition_model=trans_model, sensors=[], mounting_offsets=[], - mapping=mounting_mapping + position_mapping=mounting_mapping ) for sensor, offset in zip(radars_3d, mounting_offsets_3d): platform.add_sensor(sensor, offset) @@ -412,7 +412,7 @@ def test_3d_platform(state, expected, move, radars_3d, mounting_offsets_3d, transition_model=trans_model, sensors=radars_3d, mounting_offsets=mounting_offsets_3d, - mapping=mounting_mapping + position_mapping=mounting_mapping ) if move: # Move the platform @@ -528,7 +528,7 @@ def test_rotation_offsets_2d(state, expected_platform_orientation, expected_sens [model_1d] * (radars_2d[0].ndim_state // 2)) platform_state = State(state, timestamp) - # This defines the mapping to the platforms state vector (i.e. x and y) + # This defines the position_mapping to the platforms state vector (i.e. x and y) mounting_mapping = np.array([0, 2]) # create a platform with the simple radar mounted platform = MovingSensorPlatform( @@ -536,7 +536,7 @@ def test_rotation_offsets_2d(state, expected_platform_orientation, expected_sens transition_model=trans_model, sensors=radars_2d, rotation_offsets=rotation_offsets_2d, - mapping=mounting_mapping + position_mapping=mounting_mapping ) if move: # Move the platform @@ -558,7 +558,7 @@ def test_rotation_offsets_3d(state, expected_platform_orientation, expected_sens [model_1d] * (radars_3d[0].ndim_state // 2)) platform_state = State(state, timestamp) - # This defines the mapping to the platforms state vector (i.e. x and y) + # This defines the position_mapping to the platforms state vector (i.e. x and y) mounting_mapping = np.array([0, 2, 4]) # create a platform with the simple radar mounted platform = MovingSensorPlatform( @@ -566,7 +566,7 @@ def test_rotation_offsets_3d(state, expected_platform_orientation, expected_sens transition_model=trans_model, sensors=radars_3d, rotation_offsets=rotation_offsets_3d, - mapping=mounting_mapping + position_mapping=mounting_mapping ) if move: # Move the platform @@ -594,13 +594,13 @@ def test_defaults(radars_3d, platform_type, add_sensor): platform_args['transition_model'] = None if add_sensor: - platform = platform_type(state=platform_state, sensors=[], mapping=[0, 2, 4], + platform = platform_type(state=platform_state, sensors=[], position_mapping=[0, 2, 4], **platform_args) for sensor in radars_3d: platform.add_sensor(sensor) else: - platform = platform_type(state=platform_state, sensors=radars_3d, mapping=[0, 2, 4], - **platform_args) + platform = platform_type(state=platform_state, sensors=radars_3d, + position_mapping=[0, 2, 4], **platform_args) for i, sensor in enumerate(radars_3d): assert np.array_equal(platform.mounting_offsets[i], StateVector([0, 0, 0])) @@ -620,11 +620,11 @@ def test_sensor_offset_error(radars_3d, platform_type): offsets = [offset] * (len(radars_3d) - 1) with pytest.raises(ValueError): - _ = platform_type(state=platform_state, sensors=radars_3d, mapping=[0, 2, 4], + _ = platform_type(state=platform_state, sensors=radars_3d, position_mapping=[0, 2, 4], mounting_offsets=offsets, **platform_args) with pytest.raises(ValueError): - _ = platform_type(state=platform_state, sensors=radars_3d, mapping=[0, 2, 4], + _ = platform_type(state=platform_state, sensors=radars_3d, position_mapping=[0, 2, 4], rotation_offsets=offsets, **platform_args) diff --git a/stonesoup/sensor/sensor.py b/stonesoup/sensor/sensor.py index 3f6f2802e..6d3c155e5 100644 --- a/stonesoup/sensor/sensor.py +++ b/stonesoup/sensor/sensor.py @@ -20,10 +20,11 @@ def __init__(self, *args, **kwargs): position = StateVector([0, 0, 0]) if orientation is None: orientation = StateVector([0, 0, 0]) - self._internal_platform = FixedSensorPlatform(state=State(state_vector=position), - mapping=list(range(len(position))), - orientation=orientation, - sensors=[self]) + self._internal_platform = FixedSensorPlatform( + state=State(state_vector=position), + position_mapping=list(range(len(position))), + orientation=orientation, + sensors=[self]) @property def _has_internal_platform(self): diff --git a/stonesoup/sensor/tests/test_sensor.py b/stonesoup/sensor/tests/test_sensor.py index 82a0e814f..0b4a2e86f 100644 --- a/stonesoup/sensor/tests/test_sensor.py +++ b/stonesoup/sensor/tests/test_sensor.py @@ -11,18 +11,18 @@ from stonesoup.types.state import State -class TSensor(Sensor): +class TestingSensor(Sensor): def measure(self, **kwargs): pass -class TBaseSensor(BaseSensor): +class TestingBaseSensor(BaseSensor): def measure(self, **kwargs): pass def test_sensor_position_orientation_setting(): - sensor = TSensor(position=StateVector([0, 0, 1])) + sensor = TestingSensor(position=StateVector([0, 0, 1])) assert np.array_equal(sensor.position, StateVector([0, 0, 1])) assert np.array_equal(sensor.orientation, StateVector([0, 0, 0])) sensor.position = StateVector([0, 1, 0]) @@ -31,9 +31,9 @@ def test_sensor_position_orientation_setting(): assert np.array_equal(sensor.orientation, StateVector([0, 0, 0])) position = StateVector([0, 0, 1]) - sensor = TSensor() + sensor = TestingSensor() platform_state = State(state_vector=position + 1, timestamp=datetime.datetime.now()) - platform = FixedSensorPlatform(state=platform_state, mapping=[0, 1, 2]) + platform = FixedSensorPlatform(state=platform_state, position_mapping=[0, 1, 2]) platform.add_sensor(sensor) with pytest.raises(AttributeError): sensor.position = StateVector([0, 1, 0]) @@ -42,37 +42,37 @@ def test_sensor_position_orientation_setting(): def test_default_platform(): - sensor = TSensor(position=StateVector([0, 0, 1])) + sensor = TestingSensor(position=StateVector([0, 0, 1])) assert np.array_equal(sensor.position, StateVector([0, 0, 1])) assert np.array_equal(sensor.orientation, StateVector([0, 0, 0])) - sensor = TSensor(orientation=StateVector([0, 0, 1])) + sensor = TestingSensor(orientation=StateVector([0, 0, 1])) assert np.array_equal(sensor.orientation, StateVector([0, 0, 1])) assert np.array_equal(sensor.position, StateVector([0, 0, 0])) def test_internal_platform_flag(): - sensor = TSensor(position=StateVector([0, 0, 1])) + sensor = TestingSensor(position=StateVector([0, 0, 1])) assert sensor._has_internal_platform - sensor = TSensor() + sensor = TestingSensor() assert not sensor._has_internal_platform - sensor = TBaseSensor() + sensor = TestingBaseSensor() assert not sensor._has_internal_platform def test_changing_platform_from_default(): position = StateVector([0, 0, 1]) - sensor = TSensor(position=StateVector([0, 0, 1])) + sensor = TestingSensor(position=StateVector([0, 0, 1])) platform_state = State(state_vector=position+1, timestamp=datetime.datetime.now()) - platform = FixedSensorPlatform(state=platform_state, mapping=[0, 1, 2]) + platform = FixedSensorPlatform(state=platform_state, position_mapping=[0, 1, 2]) with pytest.raises(AttributeError): platform.add_sensor(sensor) -@pytest.mark.parametrize('sensor', [TBaseSensor, TSensor]) +@pytest.mark.parametrize('sensor', [TestingBaseSensor, TestingSensor]) def test_sensor_measure(sensor): # needed for test coverage... Does no harm assert sensor().measure() is None From d18164706a7e644baf605e552b8f41121ef32862 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Tue, 14 Apr 2020 09:45:42 +0100 Subject: [PATCH 24/51] Remove acceleration property from Platform --- stonesoup/platform/base.py | 18 ------------------ stonesoup/platform/tests/test_platform_base.py | 14 -------------- 2 files changed, 32 deletions(-) diff --git a/stonesoup/platform/base.py b/stonesoup/platform/base.py index b35b58775..0e22a151f 100644 --- a/stonesoup/platform/base.py +++ b/stonesoup/platform/base.py @@ -48,11 +48,6 @@ def orientation(self): def velocity(self): raise NotImplementedError - @property - @abstractmethod - def acceleration(self): - raise NotImplementedError - @abstractmethod def is_moving(self): raise NotImplementedError @@ -77,10 +72,6 @@ def _set_position(self, value): def velocity(self): return StateVector([0] * self.ndim) - @property - def acceleration(self): - return StateVector([0] * self.ndim) - @property def is_moving(self): return False @@ -108,15 +99,6 @@ def velocity(self): except IndexError: raise AttributeError('Velocity is not defined for this platform') - @property - def acceleration(self): - # TODO docs - # TODO return zeros? - try: - return self.state_vector[self.mapping + 2] - except IndexError: - raise AttributeError('Acceleration is not defined for this platform') - @property def orientation(self): # TODO docs diff --git a/stonesoup/platform/tests/test_platform_base.py b/stonesoup/platform/tests/test_platform_base.py index 04419e097..cea00fbc6 100644 --- a/stonesoup/platform/tests/test_platform_base.py +++ b/stonesoup/platform/tests/test_platform_base.py @@ -30,7 +30,6 @@ def test_base(): # Test to ensure platform time has updated assert (platform.state.timestamp == new_timestamp) assert np.array_equal(platform.velocity, StateVector([0, 0])) - assert np.array_equal(platform.acceleration, StateVector([0, 0])) assert platform.ndim == 2 assert not platform.is_moving @@ -47,7 +46,6 @@ def test_base(): # Ensure 2d platform has not moved assert np.array_equal(platform.state.state_vector, new_statevector) assert np.array_equal(platform.velocity, StateVector([0, 0, 0])) - assert np.array_equal(platform.acceleration, StateVector([0, 0, 0])) assert platform.ndim == 3 assert not platform.is_moving @@ -73,8 +71,6 @@ def test_base(): [1]]) assert (np.array_equal(platform.state.state_vector, new_statevector)) assert np.array_equal(platform.velocity, StateVector([1, 1])) - with pytest.raises(AttributeError): - _ = platform.acceleration assert platform.ndim == 2 assert platform.is_moving @@ -103,8 +99,6 @@ def test_base(): [1]]) assert (np.array_equal(platform.state.state_vector, new_statevector)) assert np.array_equal(platform.velocity, StateVector([1, 1, 1])) - with pytest.raises(AttributeError): - _ = platform.acceleration assert platform.ndim == 3 assert platform.is_moving @@ -166,8 +160,6 @@ def test_velocity_properties(): position_mapping=[0, 2, 4]) with pytest.raises(AttributeError): _ = platform.velocity - with pytest.raises(AttributeError): - _ = platform.acceleration orientation_tests_3d = [(StateVector([0, 1, 0, 0, 0, 0]), StateVector([0, 0, 0])), @@ -286,8 +278,6 @@ def test_setting_position(): platform.position = [0, 0, 0] with pytest.raises(AttributeError): platform.velocity = [0, 0, 0] - with pytest.raises(AttributeError): - platform.acceleration = [0, 0, 0] platform_state = State(np.array([[2], [2], @@ -301,8 +291,6 @@ def test_setting_position(): with pytest.raises(AttributeError): platform.velocity = [0, 0, 0] - with pytest.raises(AttributeError): - platform.acceleration = [0, 0, 0] platform_state = State(np.array([[2], [1], @@ -331,8 +319,6 @@ def test_setting_position(): with pytest.raises(AttributeError): platform.velocity = [0, 0, 0] - with pytest.raises(AttributeError): - platform.acceleration = [0, 0, 0] # noinspection PyPropertyAccess From 6b83188733ab15f63ddbaea75279fde39e71fd0b Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Tue, 14 Apr 2020 10:57:59 +0100 Subject: [PATCH 25/51] Add platform/base.py documentation --- stonesoup/functions.py | 8 +-- stonesoup/platform/base.py | 124 +++++++++++++++++++++++++++++-------- 2 files changed, 103 insertions(+), 29 deletions(-) diff --git a/stonesoup/functions.py b/stonesoup/functions.py index c62d5cd5f..4d97d1535 100644 --- a/stonesoup/functions.py +++ b/stonesoup/functions.py @@ -569,18 +569,18 @@ def mod_elevation(x): return x -def coerce_to_valid_mapping(mapping: Union[List[int], np.ndarray, StateVector]): +def coerce_to_valid_mapping(mapping: Union[List[int], np.ndarray, StateVector]) -> np.ndarray: """Function to take a mapping and convert it to a suitable form for indexing a :class:`StateVector`. Needed because if you index a :class:`StateVector` with another :class:`StateVector` (which would be the obvious technique) you get a 3d :class:`ndarray`. The input as either a List, :class:`ndarray` or a :class:`StateVector`, all of which should - contain integers. It returns a 1d :class:`ndarray` of with `shape == (ndim,)` where `ndim` is - the number of elements in the mapping array. + contain integers. It returns a 1d :class:`ndarray` of with ``shape == (ndim,)`` where ``ndim`` + is the number of elements in the mapping array. Parameters ---------- - mapping: :class:`Union[List, np.ndarray]` + mapping: :class:`Union[List, np.ndarray, StateVector]` A mapping that needs to be formatted to a valid mapping shape Returns diff --git a/stonesoup/platform/base.py b/stonesoup/platform/base.py index 0e22a151f..142ae3582 100644 --- a/stonesoup/platform/base.py +++ b/stonesoup/platform/base.py @@ -1,4 +1,5 @@ # -*- coding: utf-8 -*- +import datetime from abc import abstractmethod, ABC from ..functions import cart2sphere, cart2pol, coerce_to_valid_mapping @@ -14,7 +15,7 @@ class Platform(Base, ABC): position_mapping = Property(np.ndarray, doc="Mapping between platform position and state dims") velocity_mapping = Property(np.ndarray, default=None, doc="Mapping between platform velocity and state dims. If not " - "set, it will default to `[m+1 for m in position_mapping]`") + "set, it will default to ``[m+1 for m in position_mapping]``") def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) @@ -23,60 +24,112 @@ def __init__(self, *args, **kwargs): self.velocity_mapping = self.position_mapping + 1 @property - def state_vector(self): + def state_vector(self) -> StateVector: + """Convenience property to return the state vector of the state.""" return self.state.state_vector @property - def position(self): + def position(self) -> StateVector: + """Return the position of the platform. + + Extracted from the state vector of the platform using the platform's + :attr:`position_mapping`. This property is settable for fixed platforms, but not for + movable ones, where the position must be set by moving the model with a transition model. + """ return self.state_vector[self.position_mapping] @position.setter - def position(self, value): + def position(self, value: StateVector) -> None: self._set_position(value) @property - def ndim(self): + def ndim(self) -> int: + """Convenience property to return the number of dimensions in which the platform operates. + + Given by the length of the :attr:`position_mapping` + """ return len(self.position_mapping) @property @abstractmethod - def orientation(self): + def orientation(self) -> StateVector: + """Return the orientation of the platform. + + Implementation is case dependent and left to the Fixed/Moving subclasses + """ raise NotImplementedError @property @abstractmethod - def velocity(self): + def velocity(self) -> StateVector: + """Return the velocity of the platform. + + Implementation is case dependent and left to the Fixed/Moving subclasses + """ raise NotImplementedError @abstractmethod - def is_moving(self): + def is_moving(self) -> bool: + """Return the ``True`` if the platform is moving, ``False`` otherwise. + """ raise NotImplementedError @abstractmethod - def move(self, timestamp): + def move(self, timestamp: datetime.datetime, **kwargs) -> None: + """Update the platform position using the :attr:`transition_model`. + + Parameters + ---------- + timestamp: :class:`datetime.datetime`, optional + A timestamp signifying when the end of the maneuver \ + (the default is ``None``) + + Notes + ----- + This methods updates the value of :attr:`position`. + + Any provided ``kwargs`` are forwarded to the :attr:`transition_model`. + + If :attr:`transition_model` or ``timestamp`` is ``None``, the method has + no effect, but will return successfully. + + """ raise NotImplementedError @abstractmethod - def _set_position(self, value): + def _set_position(self, value: StateVector) -> None: raise NotImplementedError class FixedPlatform(Platform): + """Fixed platform base class + + A platform represents a random object defined as a :class:`~.State` + with fixed (but settable) position and orientation. + + .. note:: Position and orientation are a read/write properties in this class. + """ orientation = Property(StateVector, default=StateVector([0, 0, 0]), doc='A fixed orientation of the static platform') - def _set_position(self, value): + def _set_position(self, value: StateVector) -> None: self.state_vector[self.position_mapping] = value @property - def velocity(self): + def velocity(self) -> StateVector: + """Return the velocity of the platform. + + For a fixed platform this is always a zero vector of length :attr:`ndim`. + """ return StateVector([0] * self.ndim) @property - def is_moving(self): + def is_moving(self) -> bool: return False - def move(self, timestamp): + def move(self, timestamp: datetime.datetime, **kwargs) -> None: + """For a fixed platform this method has no effect other than to update the timestamp.""" + # TODO should this be the case? # Return without moving static platforms self.state.timestamp = timestamp @@ -86,23 +139,41 @@ class MovingPlatform(Platform): A platform represents a random object defined as a :class:`~.State` that moves according to a given :class:`~.TransitionModel`. + + .. note:: Position and orientation are a read only properties in this class. """ transition_model = Property( TransitionModel, doc="Transition model") @property - def velocity(self): - # TODO docs - # TODO return zeros? + def velocity(self) -> StateVector: + """Return the velocity of the platform. + + Extracted from the state vector of the platform using the platform's + :attr:`velocity_mapping`. If the state vector is too short and does not contain the + elements specified in the :attr:`velocity_mapping` this raises an :class:`AttributeError` + """ try: return self.state_vector[self.velocity_mapping] except IndexError: raise AttributeError('Velocity is not defined for this platform') @property - def orientation(self): - # TODO docs - # TODO handle roll? + def orientation(self) -> StateVector: + """Return the orientation of the platform. + + This is defined as three-element :class:`~.StateVector` in the form ``[roll, yaw, pitch]`` + or equivalently ``[roll, bearing, elevation]``. + + The orientation of this platform is defined as along the direction of its velocity, with + roll always set to zero (as this is the angle the platform is rotated about the velocity + axis, which is not defined in this approximation). + + Notes + ----- + A non-moving platform (``self.is_moving == False``) does not have a defined orientation in + this approximations and so raises an :class:`AttributeError` + """ if not self.is_moving: raise AttributeError('Orientation of a zero-velocity moving platform is not defined') velocity = self.velocity @@ -116,10 +187,13 @@ def orientation(self): @property def is_moving(self): - # TODO docs + """Return the ``True`` if the platform is moving, ``False`` otherwise. + + Equivalent (for this class) to ``all(v == 0 for v in self.velocity)`` + """ # Note: a platform without a transition model can be given a velocity as part of it's # StateVector. It just won't move - # This inconsistency will be handled in the move logic + # This inconsistency is handled in the move logic return np.any(self.velocity != 0) def _set_position(self, value): @@ -132,15 +206,15 @@ def move(self, timestamp=None, **kwargs): ---------- timestamp: :class:`datetime.datetime`, optional A timestamp signifying when the end of the maneuver \ - (the default is `None`) + (the default is ``None``) Notes ----- This methods updates the value of :attr:`position`. - Any provided `kwargs` are forwarded to the :attr:`transition_model`. + Any provided ``kwargs`` are forwarded to the :attr:`transition_model`. - If :attr:`transition_model` or `timestamp` is `None`, the method has + If :attr:`transition_model` or ``timestamp`` is ``None``, the method has no effect, but will return successfully. """ From b57649e678fb6d3cc69369d67bd5d1a0a7a778b6 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Tue, 14 Apr 2020 11:52:06 +0100 Subject: [PATCH 26/51] Add check that default creation of a fixed platform in a Sensor does not conflict with an explicitly set platform. --- stonesoup/sensor/sensor.py | 3 +++ stonesoup/sensor/tests/test_sensor.py | 10 ++++++++++ 2 files changed, 13 insertions(+) diff --git a/stonesoup/sensor/sensor.py b/stonesoup/sensor/sensor.py index 6d3c155e5..d0c9374e9 100644 --- a/stonesoup/sensor/sensor.py +++ b/stonesoup/sensor/sensor.py @@ -15,6 +15,9 @@ def __init__(self, *args, **kwargs): self._internal_platform = None super().__init__(*args, **kwargs) if position is not None or orientation is not None: + if self.platform_system is not None: + raise ValueError('Platform system and position/orientation cannot both be ' + 'specified.') if position is None: # assuming 3d for a default platform position = StateVector([0, 0, 0]) diff --git a/stonesoup/sensor/tests/test_sensor.py b/stonesoup/sensor/tests/test_sensor.py index 0b4a2e86f..32da9c335 100644 --- a/stonesoup/sensor/tests/test_sensor.py +++ b/stonesoup/sensor/tests/test_sensor.py @@ -1,4 +1,5 @@ import datetime +import weakref import pytest @@ -50,6 +51,15 @@ def test_default_platform(): assert np.array_equal(sensor.orientation, StateVector([0, 0, 1])) assert np.array_equal(sensor.position, StateVector([0, 0, 0])) + platform = FixedSensorPlatform(state=State(StateVector([0, 0, 0])), + position_mapping=StateVector([0, 1, 2])) + with pytest.raises(ValueError): + _ = TestingSensor(position=StateVector([0, 0, 1]), platform_system=weakref.ref(platform)) + + with pytest.raises(ValueError): + _ = TestingSensor(orientation=StateVector([0, 0, 1]), + platform_system=weakref.ref(platform)) + def test_internal_platform_flag(): sensor = TestingSensor(position=StateVector([0, 0, 1])) From d2ff222c66f7d8b3e986a6fab8ed5bcbbe7af6fd Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Tue, 14 Apr 2020 11:59:03 +0100 Subject: [PATCH 27/51] Add BaseSensor and Sensor documentation --- docs/source/stonesoup.sensor.rst | 3 +++ stonesoup/platform/base.py | 7 ++++-- stonesoup/sensor/base.py | 40 +++++++++++++++++++++----------- stonesoup/sensor/sensor.py | 11 +++++++++ 4 files changed, 45 insertions(+), 16 deletions(-) diff --git a/docs/source/stonesoup.sensor.rst b/docs/source/stonesoup.sensor.rst index d35f62824..73dd1aced 100644 --- a/docs/source/stonesoup.sensor.rst +++ b/docs/source/stonesoup.sensor.rst @@ -10,6 +10,9 @@ Sensors .. automodule:: stonesoup.sensor.base :show-inheritance: +.. automodule:: stonesoup.sensor.sensor + :show-inheritance: + Passive ------- diff --git a/stonesoup/platform/base.py b/stonesoup/platform/base.py index 142ae3582..1c6846020 100644 --- a/stonesoup/platform/base.py +++ b/stonesoup/platform/base.py @@ -162,8 +162,11 @@ def velocity(self) -> StateVector: def orientation(self) -> StateVector: """Return the orientation of the platform. - This is defined as three-element :class:`~.StateVector` in the form ``[roll, yaw, pitch]`` - or equivalently ``[roll, bearing, elevation]``. + This is defined as a 3x1 StateVector of angles (rad), specifying the sensor orientation in + terms of the counter-clockwise rotation around each Cartesian axis in the order + :math:`x,y,z`. The rotation angles are positive if the rotation is in the counter-clockwise + direction when viewed by an observer looking along the respective rotation axis, + towards the origin. The orientation of this platform is defined as along the direction of its velocity, with roll always set to zero (as this is the angle the platform is rotated about the velocity diff --git a/stonesoup/sensor/base.py b/stonesoup/sensor/base.py index abaa1f3ef..140ad58d9 100644 --- a/stonesoup/sensor/base.py +++ b/stonesoup/sensor/base.py @@ -1,6 +1,8 @@ # -*- coding: utf-8 -*- +import weakref from abc import abstractmethod, ABC +from ..types.array import StateVector from ..platform import Platform from ..base import Base, Property @@ -9,15 +11,19 @@ class BaseSensor(Base, ABC): """Sensor base class - A sensor object that operates according to a given - :class:`~.MeasurementModel`. + .. warning:: + This class is private and should not be used or subclassed directly. Instead use the + :class:`~.Sensor` class which is needed to achieve the functionality described in this + class's documentation. + """ - platform_system = Property(Platform, default=None, - doc='`weakref` to the platform on which the ' - 'sensor is mounted') + platform_system = Property(weakref.ref, default=None, + doc='``weakref`` to the platform on which the sensor is mounted') @property - def platform(self): + def platform(self) -> Platform: + """Return the platform system to which the sensor is attached. Resolves the ``weakref`` + stored in the :attr:`platform_system` Property.""" return self.platform_system() # noinspection PyPropertyDefinition @@ -34,12 +40,15 @@ def measure(self, **kwargs): raise NotImplementedError @property - def position(self): - """The sensor position on a 3D Cartesian plane, expressed as a 3x1 array of Cartesian - coordinates in the order :math:`x,y,z` in the order :math:`x,y,z`. + def position(self) -> StateVector: + """The sensor position on a 3D Cartesian plane, expressed as a 3x1 :class:`StateVector` + of Cartesian coordinates in the order :math:`x,y,z`. + + .. note:: + This property delegates the actual calculation of position to the platform on which the + sensor is mounted. - This property delegates that actual calculation of position to the platform on which the - sensor is mounted.""" + It is settable if, and only if, the sensor holds its own internal platform.""" return self.platform_system().get_sensor_position(self) @position.setter @@ -52,14 +61,17 @@ def position(self, value): @property def orientation(self): - """A 3x1 array of angles (rad), specifying the sensor orientation in terms of the + """A 3x1 StateVector of angles (rad), specifying the sensor orientation in terms of the counter-clockwise rotation around each Cartesian axis in the order :math:`x,y,z`. The rotation angles are positive if the rotation is in the counter-clockwise direction when viewed by an observer looking along the respective rotation axis, towards the origin. - This property delegates that actual calculation of orientation to the platform on which the - sensor is mounted.""" + .. note:: + This property delegates the actual calculation of orientation to the platform on which the + sensor is mounted. + + It is settable if, and only if, the sensor holds its own internal platform.""" return self.platform_system().get_sensor_orientation(self) @orientation.setter diff --git a/stonesoup/sensor/sensor.py b/stonesoup/sensor/sensor.py index d0c9374e9..f6400ef6c 100644 --- a/stonesoup/sensor/sensor.py +++ b/stonesoup/sensor/sensor.py @@ -7,6 +7,17 @@ class Sensor(BaseSensor, ABC): + """Sensor Base class for general use. + + Most properties and methods are inherited from :class:`~.BaseSensor`, but this class includes + crucial functionality and so should be used in preference. + + All sensors must be mounted on a platform to calculate their position and orientation. To + make this easier, if the sensor has a position and/or orientation specified in the constructor, + and no :attr:`platform_system`, then the default is to create an internally held "private" + platform for the Sensor. This restricts the later setting of the :attr:`platform_system` but + does allow the Sensor to control (and set) its own position and orientation. + """ # this functionality requires knowledge of FixedSensorPlatform so cannot go in the BaseSensor # class def __init__(self, *args, **kwargs): From 29565bbd06cbf961710de4de68780330f7c63f26 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Tue, 14 Apr 2020 15:41:28 +0100 Subject: [PATCH 28/51] Sensor platform docs --- stonesoup/platform/base.py | 2 +- stonesoup/platform/simple.py | 101 +++++++++++++++++++++++++---------- stonesoup/sensor/base.py | 4 +- 3 files changed, 75 insertions(+), 32 deletions(-) diff --git a/stonesoup/platform/base.py b/stonesoup/platform/base.py index 1c6846020..a2b02bdf8 100644 --- a/stonesoup/platform/base.py +++ b/stonesoup/platform/base.py @@ -129,7 +129,7 @@ def is_moving(self) -> bool: def move(self, timestamp: datetime.datetime, **kwargs) -> None: """For a fixed platform this method has no effect other than to update the timestamp.""" - # TODO should this be the case? + # TODO Is this a sensible implementation? # Return without moving static platforms self.state.timestamp = timestamp diff --git a/stonesoup/platform/simple.py b/stonesoup/platform/simple.py index a6dfa2c9c..33b29c59b 100644 --- a/stonesoup/platform/simple.py +++ b/stonesoup/platform/simple.py @@ -1,6 +1,6 @@ # -*- coding: utf-8 -*- from abc import ABC -from typing import List +from typing import List, Optional import numpy as np from math import cos, sin @@ -16,7 +16,7 @@ class SensorPlatformMixin(Platform, ABC): - """A simple Platform that can carry a number of different sensors and is + """A platform mixin that can carry a number of different sensors and is capable of moving based upon the :class:`~.TransitionModel`. The location of platform mounted sensors will be maintained relative to @@ -27,6 +27,12 @@ class SensorPlatformMixin(Platform, ABC): velocity. It does not take into account issues such as bank angle or body deformation (e.g. flex). + + .. note:: This class abstract and not intended to be instantiated. To get the behaviour of + this class use a subclass which combines this class with the `Platform` movement + behaviours. Currently these are :class:`~.FixedSensorPlatform` and + :class:`~.MovingSensorPlatform` + """ sensors = Property([BaseSensor], doc="A list of N mounted sensors", default=[]) @@ -34,12 +40,12 @@ class SensorPlatformMixin(Platform, ABC): doc="A list of StateVectors containing the sensor translation " "offsets from the platform's reference point. Defaults to " "a zero vector with the same length as the Platform's " - "position_mapping") + ":attr:`position_mapping`") rotation_offsets = Property(List[StateVector], default=None, - doc="A list of StateVectors containing the sensor translation " + doc="A list of StateVectors containing the sensor rotation " "offsets from the platform's primary axis (defined as the " "direction of motion). Defaults to a zero vector with the " - "same length as the Platform's position_mapping") + "same length as the Platform's :attr:`position_mapping`") # TODO: Determine where a platform coordinate frame should be maintained @@ -70,16 +76,21 @@ def __init__(self, *args, **kwargs): for sensor in self.sensors: sensor.platform_system = weakref.ref(self) - def add_sensor(self, sensor: BaseSensor, mounting_offset: StateVector = None, - rotation_offset: StateVector = None): - """ TODO - Parameters - ---------- - sensor : :class:`stonesoup.sensor.sensor.Sensor` - The sensor object to add - mounting_offset : :class:`StateVector` - A 1xN array with the mounting offset of the new sensor - """ + def add_sensor(self, sensor: BaseSensor, mounting_offset: Optional[StateVector] = None, + rotation_offset: Optional[StateVector] = None) -> None: + """ Add a sensor to the platform + + Parameters + ---------- + sensor : :class:`~stonesoup.sensor.sensor.BaseSensor` + The sensor object to add + mounting_offset : :class:`StateVector`, optional + A StateVector with the mounting offset of the new sensor. If not supplied, defaults to + a zero vector + rotation_offset : :class:`StateVector`, optional + A StateVector with the rotation offset of the new sensor. If not supplied, defaults to + a zero vector. + """ self.sensors.append(sensor) sensor.platform_system = weakref.ref(self) @@ -91,8 +102,20 @@ def add_sensor(self, sensor: BaseSensor, mounting_offset: StateVector = None, self.mounting_offsets.append(mounting_offset) self.rotation_offsets.append(rotation_offset) - def get_sensor_position(self, sensor: BaseSensor): - # TODO docs + def get_sensor_position(self, sensor: BaseSensor) -> StateVector: + """Return the position of the given sensor, which should be already attached to the + platform. + + Parameters + ---------- + sensor : :class:`~.BaseSensor` + The sensor for which to return the position. + Returns + ------- + : :class:`StateVector` + The position of the sensor, taking into account the platform position and orientation + and the mounting offset of the sensor. + """ i = self.sensors.index(sensor) if self.is_moving: offset = self._get_rotated_offset(i) @@ -101,25 +124,37 @@ def get_sensor_position(self, sensor: BaseSensor): new_sensor_pos = self.position + offset return new_sensor_pos - def get_sensor_orientation(self, sensor: BaseSensor): - # TODO docs + def get_sensor_orientation(self, sensor: BaseSensor) -> StateVector: + """Return the orientation of the given sensor, which should be already attached to the + platform. + + Parameters + ---------- + sensor : :class:`~.BaseSensor` + The sensor for which to return the orientation. + Returns + ------- + : :class:`StateVector` + The orientation of the sensor, taking into account the platform orientation + and the rotation offset of the sensor. + """ # TODO handle roll? i = self.sensors.index(sensor) offset = self.rotation_offsets[i] return self.orientation + offset - def _get_rotated_offset(self, i): + def _get_rotated_offset(self, i: int) -> np.ndarray: """ Determine the sensor mounting offset for the platforms relative orientation. Parameters ---------- - i : int + i : :class:`int` Integer reference to the sensor index Returns ------- - np.ndarray + : :class:`np.ndarray` Sensor mounting offset rotated relative to platform motion """ @@ -130,14 +165,22 @@ def _get_rotated_offset(self, i): class FixedSensorPlatform(SensorPlatformMixin, FixedPlatform): + """ A moving sensor platform that simply combines the functionality of the + :class:`~.FixedPlatform` with the :class:`~.SensorPlatformMixin`. This and + :class:`~.MovingSensorPlatform` are the primary user facing classes for platforms. + """ pass class MovingSensorPlatform(SensorPlatformMixin, MovingPlatform): + """ A moving sensor platform that simply combines the functionality of the + :class:`~.MovingPlatform` with the :class:`~.SensorPlatformMixin`. This and + :class:`~.FixedSensorPlatform` are the primary user facing classes for platforms. + """ pass -def _get_rotation_matrix(vel): +def _get_rotation_matrix(vel: StateVector) -> np.ndarray: """ Generates a rotation matrix which can be used to determine the corrected sensor offsets. @@ -150,7 +193,7 @@ def _get_rotation_matrix(vel): Parameters ---------- - vel : np.ndarrary + vel : StateVector 1xD vector denoting platform velocity in D dimensions Returns @@ -168,14 +211,14 @@ def _get_rotation_matrix(vel): [sin(theta), cos(theta)]]) -def _get_angle(vec, axis): +def _get_angle(vec: StateVector, axis: np.ndarray) -> float: """ Returns the angle between a pair of vectors. Used to determine the angle of rotation required between relative rectangular cartesian coordinate frame of reference and platform inertial frame of reference. Parameters ---------- - vec : np.ndarray + vec : StateVector 1xD array denoting platform velocity axis : np.ndarray Dx1 array denoting sensor offset relative to platform @@ -191,7 +234,7 @@ def _get_angle(vec, axis): return np.arccos(np.clip(np.dot(axis_norm, vel_norm), -1.0, 1.0)) -def _rot3d(vec): +def _rot3d(vec: np.ndarray) -> np.ndarray: """ This approach determines the platforms attitude based upon its velocity component. It does not take into account potential platform roll, nor @@ -205,7 +248,7 @@ def _rot3d(vec): Parameters ---------- - vec: np.ndarray + vec: StateVector platform velocity Returns @@ -217,7 +260,7 @@ def _rot3d(vec): @lru_cache(maxsize=128) -def _rot3d_tuple(vec): +def _rot3d_tuple(vec: tuple) -> np.ndarray: """ Private method. Should not be called directly, only from `_rot3d` Params and returns as :func:`~_rot3d` diff --git a/stonesoup/sensor/base.py b/stonesoup/sensor/base.py index 140ad58d9..f7be4e52b 100644 --- a/stonesoup/sensor/base.py +++ b/stonesoup/sensor/base.py @@ -68,8 +68,8 @@ def orientation(self): towards the origin. .. note:: - This property delegates the actual calculation of orientation to the platform on which the - sensor is mounted. + This property delegates the actual calculation of orientation to the platform on which + the sensor is mounted. It is settable if, and only if, the sensor holds its own internal platform.""" return self.platform_system().get_sensor_orientation(self) From edbe37cbfe8e7f626b656dd475066ef8a32ad76d Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Tue, 14 Apr 2020 15:47:37 +0100 Subject: [PATCH 29/51] Add test and docs for the case where a platform is asked about a missing sensor --- stonesoup/platform/simple.py | 4 ++-- .../platform/tests/test_platform_simple.py | 20 +++++++++++++++++++ 2 files changed, 22 insertions(+), 2 deletions(-) diff --git a/stonesoup/platform/simple.py b/stonesoup/platform/simple.py index 33b29c59b..579a8d68f 100644 --- a/stonesoup/platform/simple.py +++ b/stonesoup/platform/simple.py @@ -104,7 +104,7 @@ def add_sensor(self, sensor: BaseSensor, mounting_offset: Optional[StateVector] def get_sensor_position(self, sensor: BaseSensor) -> StateVector: """Return the position of the given sensor, which should be already attached to the - platform. + platform. If the sensor is not attached to the platform, raises a :class:`ValueError`. Parameters ---------- @@ -126,7 +126,7 @@ def get_sensor_position(self, sensor: BaseSensor) -> StateVector: def get_sensor_orientation(self, sensor: BaseSensor) -> StateVector: """Return the orientation of the given sensor, which should be already attached to the - platform. + platform. If the sensor is not attached to the platform, raises a :class:`ValueError`. Parameters ---------- diff --git a/stonesoup/platform/tests/test_platform_simple.py b/stonesoup/platform/tests/test_platform_simple.py index 9e4bd0dc8..ec4ff3a0c 100644 --- a/stonesoup/platform/tests/test_platform_simple.py +++ b/stonesoup/platform/tests/test_platform_simple.py @@ -628,6 +628,26 @@ def test_sensor_offset_error(radars_3d, platform_type): rotation_offsets=offsets, **platform_args) +def test_missing_sensors(radars_3d, platform_type): + platform_state = State(state_vector=StateVector([0, 1, 2, 1, 4, 1]), + timestamp=datetime.datetime.now()) + platform_args = {} + if platform_type is MovingSensorPlatform: + platform_args['transition_model'] = None + + # add all but the last sensor + platform = platform_type(state=platform_state, sensors=radars_3d[:-2], + position_mapping=[0, 2, 4], **platform_args) + + # finding the position/orientation of a sensor that is not on the platform + # should raise an error + with pytest.raises(ValueError): + platform.get_sensor_position(radars_3d[-1]) + + with pytest.raises(ValueError): + platform.get_sensor_orientation(radars_3d[-1]) + + def sensor_positions_test(expected_offset, platform): """ This function asserts that the sensor positions on the platform have been From 7c44e2edc2e2e6e79b061b64ce274cf8821e8770 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Tue, 14 Apr 2020 15:50:04 +0100 Subject: [PATCH 30/51] Clean up imports --- stonesoup/platform/tests/test_platform_simple.py | 5 ++--- stonesoup/sensor/sensor.py | 8 ++++---- 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/stonesoup/platform/tests/test_platform_simple.py b/stonesoup/platform/tests/test_platform_simple.py index ec4ff3a0c..20b5ba9dd 100644 --- a/stonesoup/platform/tests/test_platform_simple.py +++ b/stonesoup/platform/tests/test_platform_simple.py @@ -4,10 +4,9 @@ import numpy as np import pytest -from stonesoup.platform.simple import FixedSensorPlatform -from stonesoup.platform.tests import test_platform_base +from ..tests import test_platform_base from ...types.state import State -from ...platform.simple import MovingSensorPlatform +from ..simple import MovingSensorPlatform, FixedSensorPlatform from ...models.transition.linear import ( ConstantVelocity, CombinedLinearGaussianTransitionModel) from ...sensor.radar.radar import RadarRangeBearing diff --git a/stonesoup/sensor/sensor.py b/stonesoup/sensor/sensor.py index f6400ef6c..eda27b72e 100644 --- a/stonesoup/sensor/sensor.py +++ b/stonesoup/sensor/sensor.py @@ -1,9 +1,9 @@ from abc import ABC -from stonesoup.platform.simple import FixedSensorPlatform -from stonesoup.sensor.base import BaseSensor -from stonesoup.types.array import StateVector -from stonesoup.types.state import State +from ..platform.simple import FixedSensorPlatform +from .base import BaseSensor +from ..types.array import StateVector +from ..types.state import State class Sensor(BaseSensor, ABC): From 2bdc4afe574af027a974abec6e59245008f6fe46 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Wed, 15 Apr 2020 16:02:14 +0100 Subject: [PATCH 31/51] Add a couple of missing type hints as per Rich Green's comments --- stonesoup/platform/base.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/stonesoup/platform/base.py b/stonesoup/platform/base.py index a2b02bdf8..7c4d0abda 100644 --- a/stonesoup/platform/base.py +++ b/stonesoup/platform/base.py @@ -189,7 +189,7 @@ def orientation(self) -> StateVector: return StateVector([0, bearing]) @property - def is_moving(self): + def is_moving(self) -> bool: """Return the ``True`` if the platform is moving, ``False`` otherwise. Equivalent (for this class) to ``all(v == 0 for v in self.velocity)`` @@ -202,7 +202,7 @@ def is_moving(self): def _set_position(self, value): raise AttributeError('Cannot set the position of a moving platform') - def move(self, timestamp=None, **kwargs): + def move(self, timestamp=None, **kwargs) -> None: """Propagate the platform position using the :attr:`transition_model`. Parameters From f36f825ed40af3f28c8bde59e231b2da9cdea9c2 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Wed, 15 Apr 2020 16:24:22 +0100 Subject: [PATCH 32/51] Updaet docs as per Rich Green's comments --- stonesoup/functions.py | 5 +++-- stonesoup/platform/base.py | 13 ++++++++++--- stonesoup/platform/simple.py | 6 +++--- stonesoup/types/array.py | 14 +++++++++----- 4 files changed, 25 insertions(+), 13 deletions(-) diff --git a/stonesoup/functions.py b/stonesoup/functions.py index 4d97d1535..205939bd9 100644 --- a/stonesoup/functions.py +++ b/stonesoup/functions.py @@ -572,9 +572,10 @@ def mod_elevation(x): def coerce_to_valid_mapping(mapping: Union[List[int], np.ndarray, StateVector]) -> np.ndarray: """Function to take a mapping and convert it to a suitable form for indexing a :class:`StateVector`. Needed because if you index a :class:`StateVector` with another - :class:`StateVector` (which would be the obvious technique) you get a 3d :class:`ndarray`. + :class:`StateVector` (which could seem like the obvious thing to a user - it's the right shape) + you get a 3d :class:`ndarray`. - The input as either a List, :class:`ndarray` or a :class:`StateVector`, all of which should + The input is either a List, :class:`ndarray` or a :class:`StateVector`, all of which should contain integers. It returns a 1d :class:`ndarray` of with ``shape == (ndim,)`` where ``ndim`` is the number of elements in the mapping array. diff --git a/stonesoup/platform/base.py b/stonesoup/platform/base.py index 7c4d0abda..b4510d0ef 100644 --- a/stonesoup/platform/base.py +++ b/stonesoup/platform/base.py @@ -1,6 +1,7 @@ # -*- coding: utf-8 -*- import datetime from abc import abstractmethod, ABC +from typing import Sequence from ..functions import cart2sphere, cart2pol, coerce_to_valid_mapping from ..types.array import StateVector @@ -11,9 +12,15 @@ class Platform(Base, ABC): - state = Property(State, doc="The platform state at any given point") - position_mapping = Property(np.ndarray, doc="Mapping between platform position and state dims") - velocity_mapping = Property(np.ndarray, default=None, + state = Property(State, doc="The platform state at any given point. For a static platform, " + "this would usually contain its position coordinates in the form" + "``[x, y, z]``. For a moving platform it would contain position " + "and velocity interleaved: ``[x, vx, y, vy, z, vz]``") + position_mapping = Property(Sequence[int], + doc="Mapping between platform position and state vector. For a " + "position-only 3d platform this might be ``[0, 1, 2]``. For a " + "position and velocity platform: ``[0, 2, 4]``") + velocity_mapping = Property(Sequence[int], default=None, doc="Mapping between platform velocity and state dims. If not " "set, it will default to ``[m+1 for m in position_mapping]``") diff --git a/stonesoup/platform/simple.py b/stonesoup/platform/simple.py index 579a8d68f..a5d15e037 100644 --- a/stonesoup/platform/simple.py +++ b/stonesoup/platform/simple.py @@ -82,12 +82,12 @@ def add_sensor(self, sensor: BaseSensor, mounting_offset: Optional[StateVector] Parameters ---------- - sensor : :class:`~stonesoup.sensor.sensor.BaseSensor` + sensor : :class:`~.BaseSensor` The sensor object to add - mounting_offset : :class:`StateVector`, optional + mounting_offset : :class:`~.StateVector`, optional A StateVector with the mounting offset of the new sensor. If not supplied, defaults to a zero vector - rotation_offset : :class:`StateVector`, optional + rotation_offset : :class:`~.StateVector`, optional A StateVector with the rotation offset of the new sensor. If not supplied, defaults to a zero vector. """ diff --git a/stonesoup/types/array.py b/stonesoup/types/array.py index 80ee741ad..a520026fc 100644 --- a/stonesoup/types/array.py +++ b/stonesoup/types/array.py @@ -41,14 +41,18 @@ def __rmatmul__(self, other): class StateVector(Matrix): - """State vector wrapper for :class:`numpy.ndarray` + r"""State vector wrapper for :class:`numpy.ndarray` This class returns a view to a :class:`numpy.ndarray`, but ensures that - its initialised at a *Nx1* vector. It's called same as to + its initialised as an :math:`N \times 1` vector. It's called same as :func:`numpy.asarray`. The StateVector will attempt to convert the data - given to a Nx1 vector if it can easily be done. E.g., - StateVector([1., 2., 3.]), StateVector ([[1., 2., 3.,]]), and - StateVector([[1.], [2.], [3.]]) will all return the same 3x1 StateVector. + given to a :math:`N \times 1` vector if it can easily be done. E.g., + ``StateVector([1., 2., 3.])``, ``StateVector ([[1., 2., 3.,]])``, and + ``StateVector([[1.], [2.], [3.]])`` will all return the same 3x1 StateVector. + + .. note :: + It is not recommended to use a StateVector for indexing another vector. Doing so will lead + to unexpected effects. Use a :class:`tuple`, :class:`list` or :class:`np.ndarray` for this. """ def __new__(cls, *args, **kwargs): From c2230f48046d188cd9859ce7737fab89e0d265e2 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Wed, 15 Apr 2020 17:00:50 +0100 Subject: [PATCH 33/51] Doc changes to address Steve Hiscocks' comments --- stonesoup/functions.py | 6 +++--- stonesoup/platform/simple.py | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/stonesoup/functions.py b/stonesoup/functions.py index 205939bd9..2844ab17b 100644 --- a/stonesoup/functions.py +++ b/stonesoup/functions.py @@ -572,8 +572,8 @@ def mod_elevation(x): def coerce_to_valid_mapping(mapping: Union[List[int], np.ndarray, StateVector]) -> np.ndarray: """Function to take a mapping and convert it to a suitable form for indexing a :class:`StateVector`. Needed because if you index a :class:`StateVector` with another - :class:`StateVector` (which could seem like the obvious thing to a user - it's the right shape) - you get a 3d :class:`ndarray`. + :class:`StateVector` (which could seem like the obvious thing to a naive user - it's the + right shape) you get a 3d :class:`ndarray`. The input is either a List, :class:`ndarray` or a :class:`StateVector`, all of which should contain integers. It returns a 1d :class:`ndarray` of with ``shape == (ndim,)`` where ``ndim`` @@ -590,4 +590,4 @@ def coerce_to_valid_mapping(mapping: Union[List[int], np.ndarray, StateVector]) A flattened np.ndarray suitable for indexing a :class:`StateVector` """ - return np.asarray(mapping).flatten() + return np.ravel(mapping) diff --git a/stonesoup/platform/simple.py b/stonesoup/platform/simple.py index a5d15e037..1493d0933 100644 --- a/stonesoup/platform/simple.py +++ b/stonesoup/platform/simple.py @@ -194,7 +194,7 @@ def _get_rotation_matrix(vel: StateVector) -> np.ndarray: Parameters ---------- vel : StateVector - 1xD vector denoting platform velocity in D dimensions + Dx1 vector denoting platform velocity in D dimensions Returns ------- From 796660311350943db036d4bad35ce6b4b0fcdcd9 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Wed, 15 Apr 2020 17:18:05 +0100 Subject: [PATCH 34/51] Add tests of various mapping types in platforms:wq --- .../platform/tests/test_platform_base.py | 27 ++++++++++++++++++- 1 file changed, 26 insertions(+), 1 deletion(-) diff --git a/stonesoup/platform/tests/test_platform_base.py b/stonesoup/platform/tests/test_platform_base.py index cea00fbc6..26365c48a 100644 --- a/stonesoup/platform/tests/test_platform_base.py +++ b/stonesoup/platform/tests/test_platform_base.py @@ -364,8 +364,33 @@ def test_setting_orientation(): [0]]), timestamp) platform_orientation = StateVector([0, 0, 0]) - platform = FixedPlatform(state=platform_state, position_mapping=[0, 1, 2], + platform = FixedPlatform(state=platform_state, position_mapping=[0, 2, 4], orientation=platform_orientation) assert np.array_equal(platform.orientation, StateVector([0, 0, 0])) platform.orientation = StateVector([0, 1, 0]) assert np.array_equal(platform.orientation, StateVector([0, 1, 0])) + + +@pytest.mark.parametrize('mapping_type', (tuple, list, np.array)) +def test_mapping_types(mapping_type): + timestamp = datetime.datetime.now() + platform_state = State(np.array([[2], + [2], + [0]]), + timestamp) + platform = FixedPlatform(state=platform_state, position_mapping=mapping_type([0, 1, 2])) + assert np.array_equal(platform.position, StateVector([2, 2, 0])) + platform.position = StateVector([0, 0, 1]) + assert np.array_equal(platform.position, StateVector([0, 0, 1])) + + platform_state = State(np.array([[2], + [1], + [2], + [-1], + [2], + [0]]), + timestamp) + platform = MovingPlatform(state=platform_state, transition_model=None, + position_mapping=mapping_type([0, 2, 4])) + assert np.array_equal(platform.position, StateVector([2, 2, 2])) + assert np.array_equal(platform.velocity, StateVector([1, -1, 0])) From 026d9ed46176e2bd03760611b04d115700b04a9f Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Wed, 15 Apr 2020 17:21:53 +0100 Subject: [PATCH 35/51] Remove coerce_to_valid_mapping function --- stonesoup/functions.py | 27 +-------------------------- stonesoup/models/measurement/base.py | 5 ----- stonesoup/platform/base.py | 11 +++++------ stonesoup/sensor/passive.py | 5 ----- stonesoup/sensor/radar/radar.py | 10 +--------- 5 files changed, 7 insertions(+), 51 deletions(-) diff --git a/stonesoup/functions.py b/stonesoup/functions.py index 2844ab17b..db7567f27 100644 --- a/stonesoup/functions.py +++ b/stonesoup/functions.py @@ -1,12 +1,11 @@ # -*- coding: utf-8 -*- """Mathematical functions used within Stone Soup""" -from typing import List, Union import numpy as np from copy import copy from .types.numeric import Probability -from .types.array import Matrix, StateVector +from .types.array import Matrix def tria(matrix): @@ -567,27 +566,3 @@ def mod_elevation(x): elif N == 3: x = x - 2.0 * np.pi return x - - -def coerce_to_valid_mapping(mapping: Union[List[int], np.ndarray, StateVector]) -> np.ndarray: - """Function to take a mapping and convert it to a suitable form for indexing a - :class:`StateVector`. Needed because if you index a :class:`StateVector` with another - :class:`StateVector` (which could seem like the obvious thing to a naive user - it's the - right shape) you get a 3d :class:`ndarray`. - - The input is either a List, :class:`ndarray` or a :class:`StateVector`, all of which should - contain integers. It returns a 1d :class:`ndarray` of with ``shape == (ndim,)`` where ``ndim`` - is the number of elements in the mapping array. - - Parameters - ---------- - mapping: :class:`Union[List, np.ndarray, StateVector]` - A mapping that needs to be formatted to a valid mapping shape - - Returns - ------- - mapping: :class:`np.ndarray` - A flattened np.ndarray suitable for indexing a :class:`StateVector` - """ - - return np.ravel(mapping) diff --git a/stonesoup/models/measurement/base.py b/stonesoup/models/measurement/base.py index d9329fc96..bb25f08e6 100644 --- a/stonesoup/models/measurement/base.py +++ b/stonesoup/models/measurement/base.py @@ -3,7 +3,6 @@ import numpy as np -from stonesoup.functions import coerce_to_valid_mapping from ..base import Model from ...base import Property @@ -14,10 +13,6 @@ class MeasurementModel(Model, ABC): ndim_state = Property(int, doc="Number of state dimensions") mapping = Property(np.ndarray, doc="Mapping between measurement and state dims") - def __init__(self, *args, **kwargs): - super().__init__(*args, **kwargs) - self.mapping = coerce_to_valid_mapping(self.mapping) - @property def ndim(self): return self.ndim_meas diff --git a/stonesoup/platform/base.py b/stonesoup/platform/base.py index b4510d0ef..de68e2cca 100644 --- a/stonesoup/platform/base.py +++ b/stonesoup/platform/base.py @@ -3,7 +3,7 @@ from abc import abstractmethod, ABC from typing import Sequence -from ..functions import cart2sphere, cart2pol, coerce_to_valid_mapping +from ..functions import cart2sphere, cart2pol from ..types.array import StateVector from ..base import Base, Property from ..types.state import State @@ -26,9 +26,8 @@ class Platform(Base, ABC): def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) - self.position_mapping = coerce_to_valid_mapping(self.position_mapping) if self.velocity_mapping is None: - self.velocity_mapping = self.position_mapping + 1 + self.velocity_mapping = [p + 1 for p in self.position_mapping] @property def state_vector(self) -> StateVector: @@ -43,7 +42,7 @@ def position(self) -> StateVector: :attr:`position_mapping`. This property is settable for fixed platforms, but not for movable ones, where the position must be set by moving the model with a transition model. """ - return self.state_vector[self.position_mapping] + return self.state_vector[self.position_mapping, :] @position.setter def position(self, value: StateVector) -> None: @@ -120,7 +119,7 @@ class FixedPlatform(Platform): doc='A fixed orientation of the static platform') def _set_position(self, value: StateVector) -> None: - self.state_vector[self.position_mapping] = value + self.state_vector[self.position_mapping, :] = value @property def velocity(self) -> StateVector: @@ -161,7 +160,7 @@ def velocity(self) -> StateVector: elements specified in the :attr:`velocity_mapping` this raises an :class:`AttributeError` """ try: - return self.state_vector[self.velocity_mapping] + return self.state_vector[self.velocity_mapping, :] except IndexError: raise AttributeError('Velocity is not defined for this platform') diff --git a/stonesoup/sensor/passive.py b/stonesoup/sensor/passive.py index 1ea4065bf..183f0ca10 100644 --- a/stonesoup/sensor/passive.py +++ b/stonesoup/sensor/passive.py @@ -1,7 +1,6 @@ # -*- coding: utf-8 -*- import numpy as np -from stonesoup.functions import coerce_to_valid_mapping from stonesoup.sensor.sensor import Sensor from ..base import Property from ..models.measurement.nonlinear import CartesianToElevationBearing @@ -34,10 +33,6 @@ class PassiveElevationBearing(Sensor): (and follow in format) the underlying \ :class:`~.CartesianToElevationBearing` model") - def __init__(self, *args, **kwargs): - super().__init__(*args, **kwargs) - self.mapping = coerce_to_valid_mapping(self.mapping) - def measure(self, ground_truth, noise=True, **kwargs): """Generate a measurement for a given state diff --git a/stonesoup/sensor/radar/radar.py b/stonesoup/sensor/radar/radar.py index ad036951e..9b847ee7b 100644 --- a/stonesoup/sensor/radar/radar.py +++ b/stonesoup/sensor/radar/radar.py @@ -4,7 +4,7 @@ from math import erfc from stonesoup.sensor.sensor import Sensor -from ...functions import cart2sphere, rotx, roty, rotz, coerce_to_valid_mapping +from ...functions import cart2sphere, rotx, roty, rotz from ..base import Property from ...models.measurement.nonlinear import CartesianToBearingRange @@ -43,10 +43,6 @@ class RadarRangeBearing(Sensor): (and follow in format) the underlying \ :class:`~.CartesianToBearingRange` model") - def __init__(self, *args, **kwargs): - super().__init__(*args, **kwargs) - self.mapping = coerce_to_valid_mapping(self.mapping) - def measure(self, ground_truth, noise=True, **kwargs): """Generate a measurement for a given state @@ -373,10 +369,6 @@ class AESARadar(Sensor): Probability, default=1e-6, doc="Probability of false alarm used in the North's approximation") - def __init__(self, *args, **kwargs): - super().__init__(*args, **kwargs) - self.mapping = coerce_to_valid_mapping(self.mapping) - @property def _snr_constant(self): temp = 290 # noise reference temperature (room temperature kelvin) From 7543ee9ba1667dbd871bf8ef9364be7a32a2b7a4 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Wed, 15 Apr 2020 17:38:12 +0100 Subject: [PATCH 36/51] Change platform_system from Property to property. --- stonesoup/sensor/base.py | 30 ++++++++++++++++++++------- stonesoup/sensor/radar/beam_shape.py | 2 +- stonesoup/sensor/radar/radar.py | 2 +- stonesoup/sensor/sensor.py | 5 +---- stonesoup/sensor/tests/test_sensor.py | 10 --------- 5 files changed, 25 insertions(+), 24 deletions(-) diff --git a/stonesoup/sensor/base.py b/stonesoup/sensor/base.py index f7be4e52b..63da53892 100644 --- a/stonesoup/sensor/base.py +++ b/stonesoup/sensor/base.py @@ -1,11 +1,13 @@ # -*- coding: utf-8 -*- import weakref from abc import abstractmethod, ABC +from typing import Optional +from warnings import warn from ..types.array import StateVector from ..platform import Platform -from ..base import Base, Property +from ..base import Base class BaseSensor(Base, ABC): @@ -17,23 +19,35 @@ class BaseSensor(Base, ABC): class's documentation. """ - platform_system = Property(weakref.ref, default=None, - doc='``weakref`` to the platform on which the sensor is mounted') + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self._platform_system: Optional[weakref.ref] = None @property - def platform(self) -> Platform: + def platform(self) -> Optional[Platform]: """Return the platform system to which the sensor is attached. Resolves the ``weakref`` stored in the :attr:`platform_system` Property.""" - return self.platform_system() + if self.platform_system is None: + return None + else: + return self.platform_system() + + @property + def platform_system(self) -> Optional[weakref.ref]: + """Return a ``weakref`` to the platform on which the sensor is mounted""" + return self._platform_system - # noinspection PyPropertyDefinition @platform_system.setter - def set_platform_system(self, value): + def platform_system(self, value): # this slightly odd construction is to allow overriding by the Sensor subclass self._set_platform_system(value) def _set_platform_system(self, value): - self._property_platform_system = value + if self._platform_system is not None: + warn('Sensor has been moved from one platform to another. This is unexpected ' + 'behaviour') + self._platform_system = value @abstractmethod def measure(self, **kwargs): diff --git a/stonesoup/sensor/radar/beam_shape.py b/stonesoup/sensor/radar/beam_shape.py index 9526d4e33..71ef1c469 100644 --- a/stonesoup/sensor/radar/beam_shape.py +++ b/stonesoup/sensor/radar/beam_shape.py @@ -2,7 +2,7 @@ import numpy as np -from ..base import Property, Base +from ...base import Property, Base class BeamShape(Base): diff --git a/stonesoup/sensor/radar/radar.py b/stonesoup/sensor/radar/radar.py index 9b847ee7b..f988ceec9 100644 --- a/stonesoup/sensor/radar/radar.py +++ b/stonesoup/sensor/radar/radar.py @@ -5,7 +5,7 @@ from stonesoup.sensor.sensor import Sensor from ...functions import cart2sphere, rotx, roty, rotz -from ..base import Property +from ...base import Property from ...models.measurement.nonlinear import CartesianToBearingRange from ...types.array import CovarianceMatrix diff --git a/stonesoup/sensor/sensor.py b/stonesoup/sensor/sensor.py index eda27b72e..f71055a30 100644 --- a/stonesoup/sensor/sensor.py +++ b/stonesoup/sensor/sensor.py @@ -26,9 +26,6 @@ def __init__(self, *args, **kwargs): self._internal_platform = None super().__init__(*args, **kwargs) if position is not None or orientation is not None: - if self.platform_system is not None: - raise ValueError('Platform system and position/orientation cannot both be ' - 'specified.') if position is None: # assuming 3d for a default platform position = StateVector([0, 0, 0]) @@ -48,4 +45,4 @@ def _set_platform_system(self, value): if self._has_internal_platform: raise AttributeError('Platform system cannot be set on sensors that were created with ' 'a default platform') - self._property_platform_system = value + super()._set_platform_system(value) diff --git a/stonesoup/sensor/tests/test_sensor.py b/stonesoup/sensor/tests/test_sensor.py index 32da9c335..0b4a2e86f 100644 --- a/stonesoup/sensor/tests/test_sensor.py +++ b/stonesoup/sensor/tests/test_sensor.py @@ -1,5 +1,4 @@ import datetime -import weakref import pytest @@ -51,15 +50,6 @@ def test_default_platform(): assert np.array_equal(sensor.orientation, StateVector([0, 0, 1])) assert np.array_equal(sensor.position, StateVector([0, 0, 0])) - platform = FixedSensorPlatform(state=State(StateVector([0, 0, 0])), - position_mapping=StateVector([0, 1, 2])) - with pytest.raises(ValueError): - _ = TestingSensor(position=StateVector([0, 0, 1]), platform_system=weakref.ref(platform)) - - with pytest.raises(ValueError): - _ = TestingSensor(orientation=StateVector([0, 0, 1]), - platform_system=weakref.ref(platform)) - def test_internal_platform_flag(): sensor = TestingSensor(position=StateVector([0, 0, 1])) From 3f874fc136760cc70e41d79fe51c4f33ce8143e7 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Fri, 17 Apr 2020 10:01:11 +0100 Subject: [PATCH 37/51] Change orientation and position properties of sensor to make use of the platform property, rather than platform system. --- stonesoup/sensor/base.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/stonesoup/sensor/base.py b/stonesoup/sensor/base.py index 63da53892..8b26620bb 100644 --- a/stonesoup/sensor/base.py +++ b/stonesoup/sensor/base.py @@ -63,7 +63,7 @@ def position(self) -> StateVector: sensor is mounted. It is settable if, and only if, the sensor holds its own internal platform.""" - return self.platform_system().get_sensor_position(self) + return self.platform.get_sensor_position(self) @position.setter def position(self, value): @@ -86,7 +86,7 @@ def orientation(self): the sensor is mounted. It is settable if, and only if, the sensor holds its own internal platform.""" - return self.platform_system().get_sensor_orientation(self) + return self.platform.get_sensor_orientation(self) @orientation.setter def orientation(self, value): From 0961480db639d5c6114fcb686ea55f2eddfedd97 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Fri, 17 Apr 2020 10:43:02 +0100 Subject: [PATCH 38/51] Add more type hinting to sensor --- stonesoup/sensor/base.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/stonesoup/sensor/base.py b/stonesoup/sensor/base.py index 8b26620bb..0a73367b0 100644 --- a/stonesoup/sensor/base.py +++ b/stonesoup/sensor/base.py @@ -39,11 +39,11 @@ def platform_system(self) -> Optional[weakref.ref]: return self._platform_system @platform_system.setter - def platform_system(self, value): + def platform_system(self, value: weakref.ref): # this slightly odd construction is to allow overriding by the Sensor subclass self._set_platform_system(value) - def _set_platform_system(self, value): + def _set_platform_system(self, value: weakref.ref): if self._platform_system is not None: warn('Sensor has been moved from one platform to another. This is unexpected ' 'behaviour') @@ -66,7 +66,7 @@ def position(self) -> StateVector: return self.platform.get_sensor_position(self) @position.setter - def position(self, value): + def position(self, value: StateVector): if self._has_internal_platform: self.platform.position = value else: @@ -89,7 +89,7 @@ def orientation(self): return self.platform.get_sensor_orientation(self) @orientation.setter - def orientation(self, value): + def orientation(self, value: StateVector): if self._has_internal_platform: self.platform.position = value else: @@ -97,5 +97,5 @@ def orientation(self, value): 'default platform') @property - def _has_internal_platform(self): + def _has_internal_platform(self) -> bool: return False From 11730e7272d9598f871db3506546f97c1293fbed Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Fri, 17 Apr 2020 11:37:23 +0100 Subject: [PATCH 39/51] Fixes after merge of 'master' --- stonesoup/simulator/tests/conftest.py | 2 +- stonesoup/simulator/tests/test_platform.py | 15 +++++++-------- 2 files changed, 8 insertions(+), 9 deletions(-) diff --git a/stonesoup/simulator/tests/conftest.py b/stonesoup/simulator/tests/conftest.py index f7420dbb3..bc98f3583 100644 --- a/stonesoup/simulator/tests/conftest.py +++ b/stonesoup/simulator/tests/conftest.py @@ -4,7 +4,7 @@ import numpy as np from ...types.detection import Detection -from ...sensor import Sensor +from ...sensor.sensor import Sensor @pytest.fixture() diff --git a/stonesoup/simulator/tests/test_platform.py b/stonesoup/simulator/tests/test_platform.py index af3816d5c..124e87cfb 100644 --- a/stonesoup/simulator/tests/test_platform.py +++ b/stonesoup/simulator/tests/test_platform.py @@ -4,9 +4,8 @@ import numpy as np -from ...models.transition.linear import \ - CombinedLinearGaussianTransitionModel, ConstantVelocity -from ...platform.simple import SensorPlatform +from stonesoup.platform.simple import MovingSensorPlatform +from ...models.transition.linear import CombinedLinearGaussianTransitionModel, ConstantVelocity from ...types.state import State from ..platform import PlatformDetectionSimulator from ..simple import SingleTargetGroundTruthSimulator @@ -18,11 +17,11 @@ def build_platform(sensors, x_velocity): model_1d = ConstantVelocity(0.0) # zero noise so pure movement trans_model = CombinedLinearGaussianTransitionModel([model_1d] * 2) mounting_offsets = np.zeros((len(sensors), 2)) - mounting_mappings = np.array([[0, 2]]) - platform = SensorPlatform(state=state, sensors=sensors, - transition_model=trans_model, - mounting_offsets=mounting_offsets, - mounting_mappings=mounting_mappings) + position_mapping = np.array([[0, 2]]) + platform = MovingSensorPlatform(state=state, sensors=sensors, + transition_model=trans_model, + mounting_offsets=mounting_offsets, + position_mapping=position_mapping) return platform From 4813a1eb6dc4d56ee4467b81e862d558f759176e Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Fri, 17 Apr 2020 11:43:29 +0100 Subject: [PATCH 40/51] Make base platform have sensors. Simplifies the inheritance considerably, without excluding any use cases. --- stonesoup/platform/base.py | 258 +++++++++++++++- stonesoup/platform/simple.py | 282 ------------------ .../platform/tests/test_platform_simple.py | 24 +- stonesoup/sensor/sensor.py | 6 +- stonesoup/sensor/tests/test_sensor.py | 16 +- stonesoup/simulator/tests/test_platform.py | 10 +- 6 files changed, 283 insertions(+), 313 deletions(-) delete mode 100644 stonesoup/platform/simple.py diff --git a/stonesoup/platform/base.py b/stonesoup/platform/base.py index 3e82f3871..25640a9d6 100644 --- a/stonesoup/platform/base.py +++ b/stonesoup/platform/base.py @@ -1,17 +1,53 @@ # -*- coding: utf-8 -*- import datetime +import weakref from abc import abstractmethod, ABC -from typing import Sequence +from functools import lru_cache -from ..functions import cart2sphere, cart2pol +from math import cos, sin +from typing import Sequence, Optional, List, TYPE_CHECKING + +from scipy.linalg import expm +import numpy as np + +from ..functions import cart2sphere, cart2pol, rotz from ..types.array import StateVector from ..base import Base, Property from ..types.state import State from ..models.transition import TransitionModel -import numpy as np +if TYPE_CHECKING: + from ..sensor.base import BaseSensor class Platform(Base, ABC): + """A platform that can carry a number of different sensors. + + The location of platform mounted sensors will be maintained relative to + the sensor position. Platforms move within a 2 or 3 dimensional + rectangular cartesian space. + + A simple platform is considered to always be aligned with its principle + velocity. It does not take into account issues such as bank angle or body + deformation (e.g. flex). + + + .. note:: This class abstract and not intended to be instantiated. To get the behaviour of + this class use a subclass which gives movement + behaviours. Currently these are :class:`~.FixedPlatform` and + :class:`~.MovingPlatform` + + """ + rotation_offsets = Property(List[StateVector], default=None, + doc="A list of StateVectors containing the sensor rotation " + "offsets from the platform's primary axis (defined as the " + "direction of motion). Defaults to a zero vector with the " + "same length as the Platform's :attr:`position_mapping`") + mounting_offsets = Property(List[StateVector], default=None, + doc="A list of StateVectors containing the sensor translation " + "offsets from the platform's reference point. Defaults to " + "a zero vector with the same length as the Platform's " + ":attr:`position_mapping`") + sensors = Property(List["BaseSensor"], doc="A list of N mounted sensors", default=[]) state = Property(State, doc="The platform state at any given point. For a static platform, " "this would usually contain its position coordinates in the form" "``[x, y, z]``. For a moving platform it would contain position " @@ -24,11 +60,38 @@ class Platform(Base, ABC): doc="Mapping between platform velocity and state dims. If not " "set, it will default to ``[m+1 for m in position_mapping]``") + # TODO: Determine where a platform coordinate frame should be maintained + def __init__(self, *args, **kwargs): + """ + Ensure that the platform location and the sensor locations are + consistent at initialisation. + """ super().__init__(*args, **kwargs) + # Set values to defaults if not provided if self.velocity_mapping is None: self.velocity_mapping = [p + 1 for p in self.position_mapping] + if self.mounting_offsets is None: + self.mounting_offsets = [StateVector([0] * self.ndim)] * len(self.sensors) + + if self.rotation_offsets is None: + self.rotation_offsets = [StateVector([0] * 3)] * len(self.sensors) + + if len(self.sensors) != len(self.mounting_offsets): + raise ValueError( + "Number of sensors associated with the platform does not " + "match the number of sensor mounting offsets specified") + + if len(self.sensors) != len(self.rotation_offsets): + raise ValueError( + "Number of sensors associated with the platform does not " + "match the number of sensor rotation offsets specified") + + # Store the platform weakref in each of the child sensors + for sensor in self.sensors: + sensor.platform_system = weakref.ref(self) + @property def state_vector(self) -> StateVector: """Convenience property to return the state vector of the state.""" @@ -110,6 +173,93 @@ def move(self, timestamp: datetime.datetime, **kwargs) -> None: def _set_position(self, value: StateVector) -> None: raise NotImplementedError + def add_sensor(self, sensor: "BaseSensor", mounting_offset: Optional[StateVector] = None, + rotation_offset: Optional[StateVector] = None) -> None: + """ Add a sensor to the platform + + Parameters + ---------- + sensor : :class:`~.BaseSensor` + The sensor object to add + mounting_offset : :class:`~.StateVector`, optional + A StateVector with the mounting offset of the new sensor. If not supplied, defaults to + a zero vector + rotation_offset : :class:`~.StateVector`, optional + A StateVector with the rotation offset of the new sensor. If not supplied, defaults to + a zero vector. + """ + self.sensors.append(sensor) + sensor.platform_system = weakref.ref(self) + + if mounting_offset is None: + mounting_offset = StateVector([0] * self.ndim) + if rotation_offset is None: + rotation_offset = StateVector([0] * 3) + + self.mounting_offsets.append(mounting_offset) + self.rotation_offsets.append(rotation_offset) + + def get_sensor_position(self, sensor: "BaseSensor") -> StateVector: + """Return the position of the given sensor, which should be already attached to the + platform. If the sensor is not attached to the platform, raises a :class:`ValueError`. + + Parameters + ---------- + sensor : :class:`~.BaseSensor` + The sensor for which to return the position. + Returns + ------- + : :class:`StateVector` + The position of the sensor, taking into account the platform position and orientation + and the mounting offset of the sensor. + """ + i = self.sensors.index(sensor) + if self.is_moving: + offset = self._get_rotated_offset(i) + else: + offset = self.mounting_offsets[i] + new_sensor_pos = self.position + offset + return new_sensor_pos + + def get_sensor_orientation(self, sensor: "BaseSensor") -> StateVector: + """Return the orientation of the given sensor, which should be already attached to the + platform. If the sensor is not attached to the platform, raises a :class:`ValueError`. + + Parameters + ---------- + sensor : :class:`~.BaseSensor` + The sensor for which to return the orientation. + Returns + ------- + : :class:`StateVector` + The orientation of the sensor, taking into account the platform orientation + and the rotation offset of the sensor. + """ + # TODO handle roll? + i = self.sensors.index(sensor) + offset = self.rotation_offsets[i] + return self.orientation + offset + + def _get_rotated_offset(self, i: int) -> np.ndarray: + """ Determine the sensor mounting offset for the platforms relative + orientation. + + Parameters + ---------- + i : :class:`int` + Integer reference to the sensor index + + Returns + ------- + : :class:`np.ndarray` + Sensor mounting offset rotated relative to platform motion + """ + + vel = self.velocity + + rot = _get_rotation_matrix(vel) + return rot @ self.mounting_offsets[i] + class FixedPlatform(Platform): """Fixed platform base class @@ -254,3 +404,105 @@ def move(self, timestamp=None, **kwargs) -> None: time_interval=time_interval, **kwargs), timestamp=timestamp) + + +def _get_rotation_matrix(vel: StateVector) -> np.ndarray: + """ Generates a rotation matrix which can be used to determine the + corrected sensor offsets. + + In the 2d case this returns the following rotation matrix + [cos[theta] -sin[theta]] + [cos[theta] sin[theta]] + + In the 2d case this will be a 3x3 matrix which rotates around the Z axis + followed by a rotation about the new Y axis. + + Parameters + ---------- + vel : StateVector + Dx1 vector denoting platform velocity in D dimensions + + Returns + ------- + np.array + DxD rotation matrix + """ + if len(vel) == 3: + return _rot3d(vel) + elif len(vel) == 2: + theta = _get_angle(vel, np.array([[1, 0]])) + if vel[1] < 0: + theta *= -1 + return np.array([[cos(theta), -sin(theta)], + [sin(theta), cos(theta)]]) + + +def _get_angle(vec: StateVector, axis: np.ndarray) -> float: + """ Returns the angle between a pair of vectors. Used to determine the + angle of rotation required between relative rectangular cartesian + coordinate frame of reference and platform inertial frame of reference. + + Parameters + ---------- + vec : StateVector + 1xD array denoting platform velocity + axis : np.ndarray + Dx1 array denoting sensor offset relative to platform + + Returns + ------- + Angle : float + Angle, in radians, between the two vectors + """ + vel_norm = vec / np.linalg.norm(vec) + axis_norm = axis / np.linalg.norm(axis) + + return np.arccos(np.clip(np.dot(axis_norm, vel_norm), -1.0, 1.0)) + + +def _rot3d(vec: np.ndarray) -> np.ndarray: + """ + This approach determines the platforms attitude based upon its velocity + component. It does not take into account potential platform roll, nor + are the components calculated to account for physical artifacts such as + platform trim (e.g. aircraft yaw whilst flying forwards). + + The process determines the yaw (x-y) and pitch (z to x-y plane) angles. + The rotation matrix for a rotation by yaw around the Z-axis is then + calculated, the rotated Y axis is then determined and used to calculate the + rotation matrix which takes into account the platform pitch + + Parameters + ---------- + vec: StateVector + platform velocity + + Returns + ------- + np.ndarray + 3x3 rotation matrix + """ + return _rot3d_tuple(tuple(vec.flat)) + + +@lru_cache(maxsize=128) +def _rot3d_tuple(vec: tuple) -> np.ndarray: + """ Private method. Should not be called directly, only from `_rot3d` + + Params and returns as :func:`~_rot3d` + + This wrapped method takes a tuple rather than a state vector. This allows caching, which + is important as the new sensor approach means `_rot3d` is called on each call to get_position, + and becomes a significant performance hit. + + """ + # TODO handle platform roll + yaw = np.arctan2(vec[1], vec[0]) + pitch = np.arctan2(vec[2], + np.sqrt(vec[0] ** 2 + vec[1] ** 2)) * -1 + rot_z = rotz(yaw) + # Modify to correct for new y axis + y_axis = np.array([0, 1, 0]) + rot_y = expm(np.cross(np.eye(3), np.dot(rot_z, y_axis) * pitch)) + + return np.dot(rot_y, rot_z) diff --git a/stonesoup/platform/simple.py b/stonesoup/platform/simple.py deleted file mode 100644 index 0994f5689..000000000 --- a/stonesoup/platform/simple.py +++ /dev/null @@ -1,282 +0,0 @@ -# -*- coding: utf-8 -*- -from abc import ABC -from typing import List, Optional - -import numpy as np -from math import cos, sin -from scipy.linalg import expm -import weakref -from functools import lru_cache - -from ..sensor.base import BaseSensor -from ..functions import rotz -from ..base import Property -from ..types.state import StateVector -from .base import Platform, MovingPlatform, FixedPlatform - - -class SensorPlatformMixin(Platform, ABC): - """A platform mixin that can carry a number of different sensors and is - capable of moving based upon the :class:`~.TransitionModel`. - - The location of platform mounted sensors will be maintained relative to - the sensor position. Simple platforms move within a 2 or 3 dimensional - rectangular cartesian space. - - A simple platform is considered to always be aligned with its principle - velocity. It does not take into account issues such as bank angle or body - deformation (e.g. flex). - - - .. note:: This class abstract and not intended to be instantiated. To get the behaviour of - this class use a subclass which combines this class with the `Platform` movement - behaviours. Currently these are :class:`~.FixedSensorPlatform` and - :class:`~.MovingSensorPlatform` - - """ - - sensors = Property(List[BaseSensor], doc="A list of N mounted sensors", default=[]) - mounting_offsets = Property(List[StateVector], default=None, - doc="A list of StateVectors containing the sensor translation " - "offsets from the platform's reference point. Defaults to " - "a zero vector with the same length as the Platform's " - ":attr:`position_mapping`") - rotation_offsets = Property(List[StateVector], default=None, - doc="A list of StateVectors containing the sensor rotation " - "offsets from the platform's primary axis (defined as the " - "direction of motion). Defaults to a zero vector with the " - "same length as the Platform's :attr:`position_mapping`") - - # TODO: Determine where a platform coordinate frame should be maintained - - def __init__(self, *args, **kwargs): - """ - Ensure that the platform location and the sensor locations are - consistent at initialisation. - """ - super().__init__(*args, **kwargs) - # Set values to defaults if not provided - if self.mounting_offsets is None: - self.mounting_offsets = [StateVector([0] * self.ndim)] * len(self.sensors) - - if self.rotation_offsets is None: - self.rotation_offsets = [StateVector([0] * 3)] * len(self.sensors) - - if len(self.sensors) != len(self.mounting_offsets): - raise ValueError( - "Number of sensors associated with the platform does not " - "match the number of sensor mounting offsets specified") - - if len(self.sensors) != len(self.rotation_offsets): - raise ValueError( - "Number of sensors associated with the platform does not " - "match the number of sensor rotation offsets specified") - - # Store the platform weakref in each of the child sensors - for sensor in self.sensors: - sensor.platform_system = weakref.ref(self) - - def add_sensor(self, sensor: BaseSensor, mounting_offset: Optional[StateVector] = None, - rotation_offset: Optional[StateVector] = None) -> None: - """ Add a sensor to the platform - - Parameters - ---------- - sensor : :class:`~.BaseSensor` - The sensor object to add - mounting_offset : :class:`~.StateVector`, optional - A StateVector with the mounting offset of the new sensor. If not supplied, defaults to - a zero vector - rotation_offset : :class:`~.StateVector`, optional - A StateVector with the rotation offset of the new sensor. If not supplied, defaults to - a zero vector. - """ - self.sensors.append(sensor) - sensor.platform_system = weakref.ref(self) - - if mounting_offset is None: - mounting_offset = StateVector([0] * self.ndim) - if rotation_offset is None: - rotation_offset = StateVector([0] * 3) - - self.mounting_offsets.append(mounting_offset) - self.rotation_offsets.append(rotation_offset) - - def get_sensor_position(self, sensor: BaseSensor) -> StateVector: - """Return the position of the given sensor, which should be already attached to the - platform. If the sensor is not attached to the platform, raises a :class:`ValueError`. - - Parameters - ---------- - sensor : :class:`~.BaseSensor` - The sensor for which to return the position. - Returns - ------- - : :class:`StateVector` - The position of the sensor, taking into account the platform position and orientation - and the mounting offset of the sensor. - """ - i = self.sensors.index(sensor) - if self.is_moving: - offset = self._get_rotated_offset(i) - else: - offset = self.mounting_offsets[i] - new_sensor_pos = self.position + offset - return new_sensor_pos - - def get_sensor_orientation(self, sensor: BaseSensor) -> StateVector: - """Return the orientation of the given sensor, which should be already attached to the - platform. If the sensor is not attached to the platform, raises a :class:`ValueError`. - - Parameters - ---------- - sensor : :class:`~.BaseSensor` - The sensor for which to return the orientation. - Returns - ------- - : :class:`StateVector` - The orientation of the sensor, taking into account the platform orientation - and the rotation offset of the sensor. - """ - # TODO handle roll? - i = self.sensors.index(sensor) - offset = self.rotation_offsets[i] - return self.orientation + offset - - def _get_rotated_offset(self, i: int) -> np.ndarray: - """ Determine the sensor mounting offset for the platforms relative - orientation. - - Parameters - ---------- - i : :class:`int` - Integer reference to the sensor index - - Returns - ------- - : :class:`np.ndarray` - Sensor mounting offset rotated relative to platform motion - """ - - vel = self.velocity - - rot = _get_rotation_matrix(vel) - return rot @ self.mounting_offsets[i] - - -class FixedSensorPlatform(SensorPlatformMixin, FixedPlatform): - """ A moving sensor platform that simply combines the functionality of the - :class:`~.FixedPlatform` with the :class:`~.SensorPlatformMixin`. This and - :class:`~.MovingSensorPlatform` are the primary user facing classes for platforms. - """ - pass - - -class MovingSensorPlatform(SensorPlatformMixin, MovingPlatform): - """ A moving sensor platform that simply combines the functionality of the - :class:`~.MovingPlatform` with the :class:`~.SensorPlatformMixin`. This and - :class:`~.FixedSensorPlatform` are the primary user facing classes for platforms. - """ - pass - - -def _get_rotation_matrix(vel: StateVector) -> np.ndarray: - """ Generates a rotation matrix which can be used to determine the - corrected sensor offsets. - - In the 2d case this returns the following rotation matrix - [cos[theta] -sin[theta]] - [cos[theta] sin[theta]] - - In the 2d case this will be a 3x3 matrix which rotates around the Z axis - followed by a rotation about the new Y axis. - - Parameters - ---------- - vel : StateVector - Dx1 vector denoting platform velocity in D dimensions - - Returns - ------- - np.array - DxD rotation matrix - """ - if len(vel) == 3: - return _rot3d(vel) - elif len(vel) == 2: - theta = _get_angle(vel, np.array([[1, 0]])) - if vel[1] < 0: - theta *= -1 - return np.array([[cos(theta), -sin(theta)], - [sin(theta), cos(theta)]]) - - -def _get_angle(vec: StateVector, axis: np.ndarray) -> float: - """ Returns the angle between a pair of vectors. Used to determine the - angle of rotation required between relative rectangular cartesian - coordinate frame of reference and platform inertial frame of reference. - - Parameters - ---------- - vec : StateVector - 1xD array denoting platform velocity - axis : np.ndarray - Dx1 array denoting sensor offset relative to platform - - Returns - ------- - Angle : float - Angle, in radians, between the two vectors - """ - vel_norm = vec / np.linalg.norm(vec) - axis_norm = axis / np.linalg.norm(axis) - - return np.arccos(np.clip(np.dot(axis_norm, vel_norm), -1.0, 1.0)) - - -def _rot3d(vec: np.ndarray) -> np.ndarray: - """ - This approach determines the platforms attitude based upon its velocity - component. It does not take into account potential platform roll, nor - are the components calculated to account for physical artifacts such as - platform trim (e.g. aircraft yaw whilst flying forwards). - - The process determines the yaw (x-y) and pitch (z to x-y plane) angles. - The rotation matrix for a rotation by yaw around the Z-axis is then - calculated, the rotated Y axis is then determined and used to calculate the - rotation matrix which takes into account the platform pitch - - Parameters - ---------- - vec: StateVector - platform velocity - - Returns - ------- - np.ndarray - 3x3 rotation matrix - """ - return _rot3d_tuple(tuple(vec.flat)) - - -@lru_cache(maxsize=128) -def _rot3d_tuple(vec: tuple) -> np.ndarray: - """ Private method. Should not be called directly, only from `_rot3d` - - Params and returns as :func:`~_rot3d` - - This wrapped method takes a tuple rather than a state vector. This allows caching, which - is important as the new sensor approach means `_rot3d` is called on each call to get_position, - and becomes a significant performance hit. - - """ - # TODO handle platform roll - yaw = np.arctan2(vec[1], vec[0]) - pitch = np.arctan2(vec[2], - np.sqrt(vec[0] ** 2 + vec[1] ** 2)) * -1 - rot_z = rotz(yaw) - # Modify to correct for new y axis - y_axis = np.array([0, 1, 0]) - rot_y = expm(np.cross(np.eye(3), np.dot(rot_z, y_axis) * pitch)) - - return np.dot(rot_y, rot_z) diff --git a/stonesoup/platform/tests/test_platform_simple.py b/stonesoup/platform/tests/test_platform_simple.py index 20b5ba9dd..73410d5de 100644 --- a/stonesoup/platform/tests/test_platform_simple.py +++ b/stonesoup/platform/tests/test_platform_simple.py @@ -6,7 +6,7 @@ from ..tests import test_platform_base from ...types.state import State -from ..simple import MovingSensorPlatform, FixedSensorPlatform +from ..base import MovingPlatform, FixedPlatform from ...models.transition.linear import ( ConstantVelocity, CombinedLinearGaussianTransitionModel) from ...sensor.radar.radar import RadarRangeBearing @@ -248,8 +248,8 @@ def mounting_offsets_3d(): return [StateVector(offset) for offset in offsets] -@pytest.fixture(params=[MovingSensorPlatform, FixedSensorPlatform], - ids=['MovingSensorPlatform', 'FixedSensorPlatform']) +@pytest.fixture(params=[MovingPlatform, FixedPlatform], + ids=['MovingPlatform', 'FixedPlatform']) def platform_type(request): return request.param @@ -327,7 +327,7 @@ def test_2d_platform(state, expected, move, radars_2d, mounting_mapping = np.array([0, 2]) # create a platform with the simple radar mounted if add_sensor: - platform = MovingSensorPlatform( + platform = MovingPlatform( state=platform_state, transition_model=trans_model, sensors=[], @@ -337,7 +337,7 @@ def test_2d_platform(state, expected, move, radars_2d, for sensor, offset in zip(radars_2d, mounting_offsets_2d): platform.add_sensor(sensor, offset) else: - platform = MovingSensorPlatform( + platform = MovingPlatform( state=platform_state, transition_model=trans_model, sensors=radars_2d, @@ -396,7 +396,7 @@ def test_3d_platform(state, expected, move, radars_3d, mounting_offsets_3d, mounting_mapping = np.array([0, 2, 4]) # create a platform with the simple radar mounted if add_sensor: - platform = MovingSensorPlatform( + platform = MovingPlatform( state=platform_state, transition_model=trans_model, sensors=[], @@ -406,7 +406,7 @@ def test_3d_platform(state, expected, move, radars_3d, mounting_offsets_3d, for sensor, offset in zip(radars_3d, mounting_offsets_3d): platform.add_sensor(sensor, offset) else: - platform = MovingSensorPlatform( + platform = MovingPlatform( state=platform_state, transition_model=trans_model, sensors=radars_3d, @@ -530,7 +530,7 @@ def test_rotation_offsets_2d(state, expected_platform_orientation, expected_sens # This defines the position_mapping to the platforms state vector (i.e. x and y) mounting_mapping = np.array([0, 2]) # create a platform with the simple radar mounted - platform = MovingSensorPlatform( + platform = MovingPlatform( state=platform_state, transition_model=trans_model, sensors=radars_2d, @@ -560,7 +560,7 @@ def test_rotation_offsets_3d(state, expected_platform_orientation, expected_sens # This defines the position_mapping to the platforms state vector (i.e. x and y) mounting_mapping = np.array([0, 2, 4]) # create a platform with the simple radar mounted - platform = MovingSensorPlatform( + platform = MovingPlatform( state=platform_state, transition_model=trans_model, sensors=radars_3d, @@ -589,7 +589,7 @@ def test_defaults(radars_3d, platform_type, add_sensor): platform_state = State(state_vector=StateVector([0, 1, 2, 1, 4, 1]), timestamp=datetime.datetime.now()) platform_args = {} - if platform_type is MovingSensorPlatform: + if platform_type is MovingPlatform: platform_args['transition_model'] = None if add_sensor: @@ -612,7 +612,7 @@ def test_sensor_offset_error(radars_3d, platform_type): platform_state = State(state_vector=StateVector([0, 1, 2, 1, 4, 1]), timestamp=datetime.datetime.now()) platform_args = {} - if platform_type is MovingSensorPlatform: + if platform_type is MovingPlatform: platform_args['transition_model'] = None offset = StateVector([0, 0, 0]) @@ -631,7 +631,7 @@ def test_missing_sensors(radars_3d, platform_type): platform_state = State(state_vector=StateVector([0, 1, 2, 1, 4, 1]), timestamp=datetime.datetime.now()) platform_args = {} - if platform_type is MovingSensorPlatform: + if platform_type is MovingPlatform: platform_args['transition_model'] = None # add all but the last sensor diff --git a/stonesoup/sensor/sensor.py b/stonesoup/sensor/sensor.py index f71055a30..41c9da45f 100644 --- a/stonesoup/sensor/sensor.py +++ b/stonesoup/sensor/sensor.py @@ -1,6 +1,6 @@ from abc import ABC -from ..platform.simple import FixedSensorPlatform +from ..platform.base import FixedPlatform from .base import BaseSensor from ..types.array import StateVector from ..types.state import State @@ -18,7 +18,7 @@ class Sensor(BaseSensor, ABC): platform for the Sensor. This restricts the later setting of the :attr:`platform_system` but does allow the Sensor to control (and set) its own position and orientation. """ - # this functionality requires knowledge of FixedSensorPlatform so cannot go in the BaseSensor + # this functionality requires knowledge of FixedPlatform so cannot go in the BaseSensor # class def __init__(self, *args, **kwargs): position = kwargs.pop('position', None) @@ -31,7 +31,7 @@ def __init__(self, *args, **kwargs): position = StateVector([0, 0, 0]) if orientation is None: orientation = StateVector([0, 0, 0]) - self._internal_platform = FixedSensorPlatform( + self._internal_platform = FixedPlatform( state=State(state_vector=position), position_mapping=list(range(len(position))), orientation=orientation, diff --git a/stonesoup/sensor/tests/test_sensor.py b/stonesoup/sensor/tests/test_sensor.py index 0b4a2e86f..b3201b5f6 100644 --- a/stonesoup/sensor/tests/test_sensor.py +++ b/stonesoup/sensor/tests/test_sensor.py @@ -2,13 +2,13 @@ import pytest -from stonesoup.platform.simple import FixedSensorPlatform -from stonesoup.sensor.base import BaseSensor -from stonesoup.sensor.sensor import Sensor -from stonesoup.types.array import StateVector -import numpy as np +from ...platform.base import FixedPlatform +from ..base import BaseSensor +from ..sensor import Sensor +from ...types.array import StateVector +from ...types.state import State -from stonesoup.types.state import State +import numpy as np class TestingSensor(Sensor): @@ -33,7 +33,7 @@ def test_sensor_position_orientation_setting(): position = StateVector([0, 0, 1]) sensor = TestingSensor() platform_state = State(state_vector=position + 1, timestamp=datetime.datetime.now()) - platform = FixedSensorPlatform(state=platform_state, position_mapping=[0, 1, 2]) + platform = FixedPlatform(state=platform_state, position_mapping=[0, 1, 2]) platform.add_sensor(sensor) with pytest.raises(AttributeError): sensor.position = StateVector([0, 1, 0]) @@ -67,7 +67,7 @@ def test_changing_platform_from_default(): sensor = TestingSensor(position=StateVector([0, 0, 1])) platform_state = State(state_vector=position+1, timestamp=datetime.datetime.now()) - platform = FixedSensorPlatform(state=platform_state, position_mapping=[0, 1, 2]) + platform = FixedPlatform(state=platform_state, position_mapping=[0, 1, 2]) with pytest.raises(AttributeError): platform.add_sensor(sensor) diff --git a/stonesoup/simulator/tests/test_platform.py b/stonesoup/simulator/tests/test_platform.py index 124e87cfb..a0c1dd4d1 100644 --- a/stonesoup/simulator/tests/test_platform.py +++ b/stonesoup/simulator/tests/test_platform.py @@ -4,7 +4,7 @@ import numpy as np -from stonesoup.platform.simple import MovingSensorPlatform +from stonesoup.platform.base import MovingPlatform from ...models.transition.linear import CombinedLinearGaussianTransitionModel, ConstantVelocity from ...types.state import State from ..platform import PlatformDetectionSimulator @@ -18,10 +18,10 @@ def build_platform(sensors, x_velocity): trans_model = CombinedLinearGaussianTransitionModel([model_1d] * 2) mounting_offsets = np.zeros((len(sensors), 2)) position_mapping = np.array([[0, 2]]) - platform = MovingSensorPlatform(state=state, sensors=sensors, - transition_model=trans_model, - mounting_offsets=mounting_offsets, - position_mapping=position_mapping) + platform = MovingPlatform(state=state, sensors=sensors, + transition_model=trans_model, + mounting_offsets=mounting_offsets, + position_mapping=position_mapping) return platform From e7995b5f387a779b899842db6fc132e77d1fd847 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Fri, 17 Apr 2020 11:56:59 +0100 Subject: [PATCH 41/51] Allow setting position of moving platforms, provided their transition model is not set. --- stonesoup/platform/base.py | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/stonesoup/platform/base.py b/stonesoup/platform/base.py index 25640a9d6..6f9a75a71 100644 --- a/stonesoup/platform/base.py +++ b/stonesoup/platform/base.py @@ -360,7 +360,15 @@ def is_moving(self) -> bool: return np.any(self.velocity != 0) def _set_position(self, value): - raise AttributeError('Cannot set the position of a moving platform') + # The logic below is this: if a moving platform is being built from (say) input + # real-world data then its transition model would not be set, and so it would be fine to + # set its position. However, if the transition model is set, then setting the position is + # both unexpected and may cause odd effects, so is forbidden + if self.transition_model is None: + self.state_vector[self.position_mapping, :] = value + else: + raise AttributeError('Cannot set the position of a moving platform with a ' + 'transition model') def move(self, timestamp=None, **kwargs) -> None: """Propagate the platform position using the :attr:`transition_model`. From 2a678271e9f730992123f128a3361d5f1a0911d7 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Fri, 17 Apr 2020 12:21:20 +0100 Subject: [PATCH 42/51] Fix tests for update of position-setting logic. --- stonesoup/platform/base.py | 2 +- .../platform/tests/test_platform_base.py | 41 +++++++++++++++++-- 2 files changed, 38 insertions(+), 5 deletions(-) diff --git a/stonesoup/platform/base.py b/stonesoup/platform/base.py index 6f9a75a71..fd3dc6fa7 100644 --- a/stonesoup/platform/base.py +++ b/stonesoup/platform/base.py @@ -359,7 +359,7 @@ def is_moving(self) -> bool: # This inconsistency is handled in the move logic return np.any(self.velocity != 0) - def _set_position(self, value): + def _set_position(self, value: StateVector): # The logic below is this: if a moving platform is being built from (say) input # real-world data then its transition model would not be set, and so it would be fine to # set its position. However, if the transition model is set, then setting the position is diff --git a/stonesoup/platform/tests/test_platform_base.py b/stonesoup/platform/tests/test_platform_base.py index 26365c48a..7a2972fcb 100644 --- a/stonesoup/platform/tests/test_platform_base.py +++ b/stonesoup/platform/tests/test_platform_base.py @@ -272,13 +272,29 @@ def test_setting_position(): [2], [0]]), timestamp) - platform = MovingPlatform(state=platform_state, transition_model=None, + model_1d = ConstantVelocity(0.0) + model_3d = CombinedLinearGaussianTransitionModel( + [model_1d, model_1d, model_1d]) + + platform = MovingPlatform(state=platform_state, transition_model=model_3d, position_mapping=[0, 1, 2]) with pytest.raises(AttributeError): platform.position = [0, 0, 0] with pytest.raises(AttributeError): platform.velocity = [0, 0, 0] + platform_state = State(np.array([[2], + [2], + [0]]), + timestamp) + platform = MovingPlatform(state=platform_state, transition_model=None, + position_mapping=[0, 1, 2]) + assert np.array_equal(platform.position, StateVector([2, 2, 0])) + platform.position = StateVector([0, 0, 0]) + assert np.array_equal(platform.position, StateVector([0, 0, 0])) + with pytest.raises(AttributeError): + platform.velocity = [0, 0, 0] + platform_state = State(np.array([[2], [2], [0]]), @@ -299,10 +315,10 @@ def test_setting_position(): [2], [0]]), timestamp) - platform = MovingPlatform(state=platform_state, transition_model=None, - position_mapping=[0, 1, 2]) + platform = MovingPlatform(state=platform_state, transition_model=model_3d, + position_mapping=[0, 2, 4]) with pytest.raises(AttributeError): - platform.position = [0, 0] + platform.position = [0, 0, 0] platform_state = State(np.array([[2], [1], @@ -320,6 +336,23 @@ def test_setting_position(): with pytest.raises(AttributeError): platform.velocity = [0, 0, 0] + platform_state = State(np.array([[2], + [1], + [2], + [-1], + [2], + [0]]), + timestamp) + platform = MovingPlatform(state=platform_state, transition_model=None, + position_mapping=[0, 2, 4]) + assert np.array_equal(platform.position, StateVector([2, 2, 2])) + platform.position = StateVector([0, 0, 1]) + assert np.array_equal(platform.position, StateVector([0, 0, 1])) + assert np.array_equal(platform.state_vector, StateVector([0, 1, 0, -1, 1, 0])) + + with pytest.raises(AttributeError): + platform.velocity = [0, 0, 0] + # noinspection PyPropertyAccess def test_setting_orientation(): From e173ac326f964d0cf41178f096f5ddd8956925a1 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Mon, 20 Apr 2020 06:24:41 +0100 Subject: [PATCH 43/51] Remove type hinting for variable: not compatible with Python 3.5 --- stonesoup/sensor/base.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/stonesoup/sensor/base.py b/stonesoup/sensor/base.py index 0a73367b0..a233aecd2 100644 --- a/stonesoup/sensor/base.py +++ b/stonesoup/sensor/base.py @@ -22,7 +22,7 @@ class BaseSensor(Base, ABC): def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) - self._platform_system: Optional[weakref.ref] = None + self._platform_system = None @property def platform(self) -> Optional[Platform]: From 927c6ecba77f0be2a9eef5194006bf218ebffacb Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Wed, 22 Apr 2020 07:56:05 +0100 Subject: [PATCH 44/51] Remove mutable defaults --- stonesoup/platform/base.py | 17 ++++++++++++++--- stonesoup/sensor/radar/beam_pattern.py | 12 +++++++++--- stonesoup/sensor/radar/radar.py | 18 +++++++++++------- 3 files changed, 34 insertions(+), 13 deletions(-) diff --git a/stonesoup/platform/base.py b/stonesoup/platform/base.py index fd3dc6fa7..e5175713b 100644 --- a/stonesoup/platform/base.py +++ b/stonesoup/platform/base.py @@ -47,7 +47,8 @@ class Platform(Base, ABC): "offsets from the platform's reference point. Defaults to " "a zero vector with the same length as the Platform's " ":attr:`position_mapping`") - sensors = Property(List["BaseSensor"], doc="A list of N mounted sensors", default=[]) + sensors = Property(List["BaseSensor"], default=None, + doc="A list of N mounted sensors. Defaults to an empty list") state = Property(State, doc="The platform state at any given point. For a static platform, " "this would usually contain its position coordinates in the form" "``[x, y, z]``. For a moving platform it would contain position " @@ -69,6 +70,10 @@ def __init__(self, *args, **kwargs): """ super().__init__(*args, **kwargs) # Set values to defaults if not provided + + if self.sensors is None: + self.sensors = [] + if self.velocity_mapping is None: self.velocity_mapping = [p + 1 for p in self.position_mapping] @@ -269,8 +274,14 @@ class FixedPlatform(Platform): .. note:: Position and orientation are a read/write properties in this class. """ - orientation = Property(StateVector, default=StateVector([0, 0, 0]), - doc='A fixed orientation of the static platform') + orientation = Property(StateVector, default=None, + doc='A fixed orientation of the static platform. ' + 'Defaults to the zero vector') + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + if self.orientation is None: + self.orientation = StateVector([0, 0, 0]) def _set_position(self, value: StateVector) -> None: self.state_vector[self.position_mapping, :] = value diff --git a/stonesoup/sensor/radar/beam_pattern.py b/stonesoup/sensor/radar/beam_pattern.py index d0cad8eb4..26436b113 100644 --- a/stonesoup/sensor/radar/beam_pattern.py +++ b/stonesoup/sensor/radar/beam_pattern.py @@ -1,5 +1,6 @@ # -*- coding: utf-8 -*- import datetime +from typing import Sequence from ...base import Property, Base @@ -39,11 +40,16 @@ class BeamSweep(BeamTransitionModel): angle_per_s = Property(float, doc="The speed that the beam scans at") frame = Property( - list, doc="Dimensions of search frame as [azimuth,elevation]") + Sequence[float], doc="Dimensions of search frame as [azimuth,elevation]") separation = Property(float, doc="Separation of lines in elevation") centre = Property( - list, default=[0, 0], - doc="Centre of the search frame in [azimuth,elevation]") + Sequence[float], default=None, + doc="Centre of the search frame in [azimuth,elevation]. Defaults to [0, 0]") + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + if self.centre is None: + self.centre = [0, 0] @property def length_frame(self): diff --git a/stonesoup/sensor/radar/radar.py b/stonesoup/sensor/radar/radar.py index 7f10d2732..fb4fff380 100644 --- a/stonesoup/sensor/radar/radar.py +++ b/stonesoup/sensor/radar/radar.py @@ -309,14 +309,13 @@ class AESARadar(Sensor): This model does not generate false alarms. """ rotation_offset = Property( - StateVector, default=StateVector([0, 0, 0]), - doc="A 3x1 array of angles (rad), specifying " - "the radar orientation in terms of the " - "counter-clockwise rotation around the " - ":math:`x,y,z` axis. i.e Roll, Pitch and Yaw.") + StateVector, default=None, + doc="A 3x1 array of angles (rad), specifying the radar orientation in terms of the " + "counter-clockwise rotation around the :math:`x,y,z` axis. i.e Roll, Pitch and Yaw. " + "Default is ``StateVector([0, 0, 0])``") mapping = Property( - np.array, default=[0, 1, 2], + np.array, default=(0, 1, 2), doc="Mapping between or positions and state " "dimensions. [x,y,z]") @@ -371,6 +370,11 @@ class AESARadar(Sensor): Probability, default=1e-6, doc="Probability of false alarm used in the North's approximation") + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + if self.rotation_offset is None: + self.rotation_offset = StateVector([0, 0, 0]) + @property def _snr_constant(self): temp = 290 # noise reference temperature (room temperature kelvin) @@ -444,7 +448,7 @@ def gen_probability(self, sky_state): spoiled_gain = 10 ** (self.antenna_gain / 10) * np.cos(beam_az) * np.cos(beam_el) spoiled_width = self.beam_width / (np.cos(beam_az) * np.cos(beam_el)) # state relative to radar (in cartesian space) - relative_vector = sky_state.state_vector[self.mapping] - self.position + relative_vector = sky_state.state_vector[self.mapping, :] - self.position relative_vector = self._rotation_matrix @ relative_vector # calculate target position in spherical coordinates From 6831a3c853f49108784c824599bb1feafeb7e877 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Wed, 22 Apr 2020 08:03:41 +0100 Subject: [PATCH 45/51] Reorder Properties of Platform to make required arguments first. --- stonesoup/platform/base.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/stonesoup/platform/base.py b/stonesoup/platform/base.py index e5175713b..c152d8bb4 100644 --- a/stonesoup/platform/base.py +++ b/stonesoup/platform/base.py @@ -37,6 +37,14 @@ class Platform(Base, ABC): :class:`~.MovingPlatform` """ + state = Property(State, doc="The platform state at any given point. For a static platform, " + "this would usually contain its position coordinates in the form" + "``[x, y, z]``. For a moving platform it would contain position " + "and velocity interleaved: ``[x, vx, y, vy, z, vz]``") + position_mapping = Property(Sequence[int], + doc="Mapping between platform position and state vector. For a " + "position-only 3d platform this might be ``[0, 1, 2]``. For a " + "position and velocity platform: ``[0, 2, 4]``") rotation_offsets = Property(List[StateVector], default=None, doc="A list of StateVectors containing the sensor rotation " "offsets from the platform's primary axis (defined as the " @@ -49,14 +57,6 @@ class Platform(Base, ABC): ":attr:`position_mapping`") sensors = Property(List["BaseSensor"], default=None, doc="A list of N mounted sensors. Defaults to an empty list") - state = Property(State, doc="The platform state at any given point. For a static platform, " - "this would usually contain its position coordinates in the form" - "``[x, y, z]``. For a moving platform it would contain position " - "and velocity interleaved: ``[x, vx, y, vy, z, vz]``") - position_mapping = Property(Sequence[int], - doc="Mapping between platform position and state vector. For a " - "position-only 3d platform this might be ``[0, 1, 2]``. For a " - "position and velocity platform: ``[0, 2, 4]``") velocity_mapping = Property(Sequence[int], default=None, doc="Mapping between platform velocity and state dims. If not " "set, it will default to ``[m+1 for m in position_mapping]``") From 29c9bd4d8005e0debce4f9eb49765c022fe377a4 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Wed, 22 Apr 2020 11:28:23 +0100 Subject: [PATCH 46/51] Handle case of missing platform in position/orientation getters --- stonesoup/sensor/base.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/stonesoup/sensor/base.py b/stonesoup/sensor/base.py index a233aecd2..2fc8514a7 100644 --- a/stonesoup/sensor/base.py +++ b/stonesoup/sensor/base.py @@ -54,7 +54,7 @@ def measure(self, **kwargs): raise NotImplementedError @property - def position(self) -> StateVector: + def position(self) -> Optional[StateVector]: """The sensor position on a 3D Cartesian plane, expressed as a 3x1 :class:`StateVector` of Cartesian coordinates in the order :math:`x,y,z`. @@ -63,6 +63,8 @@ def position(self) -> StateVector: sensor is mounted. It is settable if, and only if, the sensor holds its own internal platform.""" + if self.platform is None: + return None return self.platform.get_sensor_position(self) @position.setter @@ -74,7 +76,7 @@ def position(self, value: StateVector): 'default platform') @property - def orientation(self): + def orientation(self) -> Optional[StateVector]: """A 3x1 StateVector of angles (rad), specifying the sensor orientation in terms of the counter-clockwise rotation around each Cartesian axis in the order :math:`x,y,z`. The rotation angles are positive if the rotation is in the counter-clockwise @@ -86,6 +88,8 @@ def orientation(self): the sensor is mounted. It is settable if, and only if, the sensor holds its own internal platform.""" + if self.platform is None: + return None return self.platform.get_sensor_orientation(self) @orientation.setter From 18f431bb2dddfbbbd6052291f23d070aa8c829c1 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Wed, 22 Apr 2020 11:54:46 +0100 Subject: [PATCH 47/51] Add docs and waring on serialisation of Sensors. --- stonesoup/sensor/sensor.py | 4 ++++ stonesoup/serialise.py | 4 ++++ 2 files changed, 8 insertions(+) diff --git a/stonesoup/sensor/sensor.py b/stonesoup/sensor/sensor.py index 41c9da45f..a0055cdad 100644 --- a/stonesoup/sensor/sensor.py +++ b/stonesoup/sensor/sensor.py @@ -17,6 +17,10 @@ class Sensor(BaseSensor, ABC): and no :attr:`platform_system`, then the default is to create an internally held "private" platform for the Sensor. This restricts the later setting of the :attr:`platform_system` but does allow the Sensor to control (and set) its own position and orientation. + + .. note:: + Note that Sensors with internal platforms do not currently serialise correctly, due to the + current implementation of position and velocity as parameters to ``__init__``. """ # this functionality requires knowledge of FixedPlatform so cannot go in the BaseSensor # class diff --git a/stonesoup/serialise.py b/stonesoup/serialise.py index 8d80cfc71..bd9fbe75d 100644 --- a/stonesoup/serialise.py +++ b/stonesoup/serialise.py @@ -13,11 +13,13 @@ from functools import lru_cache from pathlib import Path from importlib import import_module +from warnings import warn import numpy as np import ruamel.yaml from ruamel.yaml.constructor import ConstructorError +from stonesoup.sensor.sensor import Sensor from .base import Base @@ -90,6 +92,8 @@ def declarative_to_yaml(cls, representer, node): Store as mapping of declared properties, skipping any which are the default value.""" + if isinstance(node, Sensor) and node._has_internal_platform: + warn('Sensors with internal platforms will not currently serialise') return representer.represent_omap( cls.yaml_tag(type(node)), OrderedDict((name, getattr(node, name)) From 4323fe1a2ca07704c600d21eb2145b4d87c82750 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Wed, 22 Apr 2020 16:24:31 +0100 Subject: [PATCH 48/51] Add special case to serialisation code for platform with internal sensor. --- stonesoup/sensor/sensor.py | 3 --- stonesoup/serialise.py | 13 ++++++++----- stonesoup/tests/test_serialise.py | 26 ++++++++++++++++++++++++++ 3 files changed, 34 insertions(+), 8 deletions(-) diff --git a/stonesoup/sensor/sensor.py b/stonesoup/sensor/sensor.py index a0055cdad..976167850 100644 --- a/stonesoup/sensor/sensor.py +++ b/stonesoup/sensor/sensor.py @@ -18,9 +18,6 @@ class Sensor(BaseSensor, ABC): platform for the Sensor. This restricts the later setting of the :attr:`platform_system` but does allow the Sensor to control (and set) its own position and orientation. - .. note:: - Note that Sensors with internal platforms do not currently serialise correctly, due to the - current implementation of position and velocity as parameters to ``__init__``. """ # this functionality requires knowledge of FixedPlatform so cannot go in the BaseSensor # class diff --git a/stonesoup/serialise.py b/stonesoup/serialise.py index bd9fbe75d..08b859af5 100644 --- a/stonesoup/serialise.py +++ b/stonesoup/serialise.py @@ -13,14 +13,14 @@ from functools import lru_cache from pathlib import Path from importlib import import_module -from warnings import warn import numpy as np import ruamel.yaml from ruamel.yaml.constructor import ConstructorError -from stonesoup.sensor.sensor import Sensor -from .base import Base +from .types.array import StateVector +from .sensor.sensor import Sensor +from .base import Base, Property class YAML: @@ -92,12 +92,15 @@ def declarative_to_yaml(cls, representer, node): Store as mapping of declared properties, skipping any which are the default value.""" + node_properties = OrderedDict(type(node).properties) + # Special case of a sensor with a default platform if isinstance(node, Sensor) and node._has_internal_platform: - warn('Sensors with internal platforms will not currently serialise') + node_properties['position'] = Property(StateVector) + node_properties['orientation'] = Property(StateVector) return representer.represent_omap( cls.yaml_tag(type(node)), OrderedDict((name, getattr(node, name)) - for name, property_ in type(node).properties.items() + for name, property_ in node_properties.items() if getattr(node, name) is not property_.default)) @classmethod diff --git a/stonesoup/tests/test_serialise.py b/stonesoup/tests/test_serialise.py index 290144d7e..b0f3f7f95 100644 --- a/stonesoup/tests/test_serialise.py +++ b/stonesoup/tests/test_serialise.py @@ -3,6 +3,8 @@ import pytest from ruamel.yaml.constructor import ConstructorError +from stonesoup.sensor.sensor import Sensor +from stonesoup.types.array import StateVector from ..serialise import YAML from ..base import Property @@ -185,3 +187,27 @@ def test_missing_property(base, serialised_file): with pytest.raises(ConstructorError, match="missing a required argument"): serialised_file.load(serialised_str) + + +def test_sensor_serialisation(serialised_file): + class TestSensor(Sensor): + def measure(self): + pass + + sensor = TestSensor() + assert sensor.position is None + assert sensor.orientation is None + serialised_str = serialised_file.dumps(sensor) + sensor = serialised_file.load(serialised_str) + assert sensor.position is None + assert sensor.orientation is None + + pos = StateVector([0, 1, 2]) + orientation = StateVector([0, np.pi/2, np.pi/4]) + sensor = TestSensor(position=pos, orientation=orientation) + assert np.allclose(sensor.position, pos) + assert np.allclose(sensor.orientation, orientation) + serialised_str = serialised_file.dumps(sensor) + sensor = serialised_file.load(serialised_str) + assert np.allclose(sensor.position, pos) + assert np.allclose(sensor.orientation, orientation) From 51fd5304d3a00a2e5e0e9e09cccb5383486fed5a Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Fri, 24 Apr 2020 18:04:17 +0100 Subject: [PATCH 49/51] Fix reassignment of platform during tests of detection simulator --- stonesoup/simulator/tests/test_platform.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/stonesoup/simulator/tests/test_platform.py b/stonesoup/simulator/tests/test_platform.py index a0c1dd4d1..86950fd6d 100644 --- a/stonesoup/simulator/tests/test_platform.py +++ b/stonesoup/simulator/tests/test_platform.py @@ -1,6 +1,7 @@ # -*- coding: utf-8 -*- import datetime +from copy import deepcopy import numpy as np @@ -91,8 +92,9 @@ def test_detection_simulator(sensor_model1, sensor_model2, transition_model1): - platform1 = build_platform([sensor_model1, sensor_model2], 1) - platform2 = build_platform([sensor_model1], 2) + # take copies of sensor_model1, otherwise sensor is TRANSFERRED from platform1 to platform2 + platform1 = build_platform([deepcopy(sensor_model1), sensor_model2], 1) + platform2 = build_platform([deepcopy(sensor_model1)], 2) initial_state = State(np.array([[0], [0], [0], [0]]), timestamp=datetime.datetime(2020, 4, 1)) From 80a759144d105a381b165772233502ab87462073 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Fri, 24 Apr 2020 18:06:35 +0100 Subject: [PATCH 50/51] Rename test sensor classes to avoid pytest warnings --- stonesoup/sensor/tests/test_sensor.py | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/stonesoup/sensor/tests/test_sensor.py b/stonesoup/sensor/tests/test_sensor.py index b3201b5f6..11d098119 100644 --- a/stonesoup/sensor/tests/test_sensor.py +++ b/stonesoup/sensor/tests/test_sensor.py @@ -11,18 +11,18 @@ import numpy as np -class TestingSensor(Sensor): +class DummySensor(Sensor): def measure(self, **kwargs): pass -class TestingBaseSensor(BaseSensor): +class DummyBaseSensor(BaseSensor): def measure(self, **kwargs): pass def test_sensor_position_orientation_setting(): - sensor = TestingSensor(position=StateVector([0, 0, 1])) + sensor = DummySensor(position=StateVector([0, 0, 1])) assert np.array_equal(sensor.position, StateVector([0, 0, 1])) assert np.array_equal(sensor.orientation, StateVector([0, 0, 0])) sensor.position = StateVector([0, 1, 0]) @@ -31,7 +31,7 @@ def test_sensor_position_orientation_setting(): assert np.array_equal(sensor.orientation, StateVector([0, 0, 0])) position = StateVector([0, 0, 1]) - sensor = TestingSensor() + sensor = DummySensor() platform_state = State(state_vector=position + 1, timestamp=datetime.datetime.now()) platform = FixedPlatform(state=platform_state, position_mapping=[0, 1, 2]) platform.add_sensor(sensor) @@ -42,29 +42,29 @@ def test_sensor_position_orientation_setting(): def test_default_platform(): - sensor = TestingSensor(position=StateVector([0, 0, 1])) + sensor = DummySensor(position=StateVector([0, 0, 1])) assert np.array_equal(sensor.position, StateVector([0, 0, 1])) assert np.array_equal(sensor.orientation, StateVector([0, 0, 0])) - sensor = TestingSensor(orientation=StateVector([0, 0, 1])) + sensor = DummySensor(orientation=StateVector([0, 0, 1])) assert np.array_equal(sensor.orientation, StateVector([0, 0, 1])) assert np.array_equal(sensor.position, StateVector([0, 0, 0])) def test_internal_platform_flag(): - sensor = TestingSensor(position=StateVector([0, 0, 1])) + sensor = DummySensor(position=StateVector([0, 0, 1])) assert sensor._has_internal_platform - sensor = TestingSensor() + sensor = DummySensor() assert not sensor._has_internal_platform - sensor = TestingBaseSensor() + sensor = DummyBaseSensor() assert not sensor._has_internal_platform def test_changing_platform_from_default(): position = StateVector([0, 0, 1]) - sensor = TestingSensor(position=StateVector([0, 0, 1])) + sensor = DummySensor(position=StateVector([0, 0, 1])) platform_state = State(state_vector=position+1, timestamp=datetime.datetime.now()) platform = FixedPlatform(state=platform_state, position_mapping=[0, 1, 2]) @@ -72,7 +72,7 @@ def test_changing_platform_from_default(): platform.add_sensor(sensor) -@pytest.mark.parametrize('sensor', [TestingBaseSensor, TestingSensor]) +@pytest.mark.parametrize('sensor', [DummyBaseSensor, DummySensor]) def test_sensor_measure(sensor): # needed for test coverage... Does no harm assert sensor().measure() is None From aac2f65f6c90f6a96a100379813b6cb306570c56 Mon Sep 17 00:00:00 2001 From: Edward TF Rogers Date: Mon, 27 Apr 2020 10:53:06 +0100 Subject: [PATCH 51/51] Add additional tests of edge cases to check defaults, errors and warnings. --- stonesoup/platform/base.py | 7 +- .../platform/tests/test_platform_base.py | 95 ++++++++++++++++++- stonesoup/sensor/tests/test_sensor.py | 10 ++ 3 files changed, 110 insertions(+), 2 deletions(-) diff --git a/stonesoup/platform/base.py b/stonesoup/platform/base.py index c152d8bb4..e64abecd3 100644 --- a/stonesoup/platform/base.py +++ b/stonesoup/platform/base.py @@ -15,7 +15,7 @@ from ..base import Base, Property from ..types.state import State from ..models.transition import TransitionModel -if TYPE_CHECKING: +if TYPE_CHECKING: # pragma: no cover from ..sensor.base import BaseSensor @@ -358,6 +358,9 @@ def orientation(self) -> StateVector: elif self.ndim == 2: _, bearing = cart2pol(*velocity.flat) return StateVector([0, bearing]) + else: + raise ValueError('Orientation of a moving platform is only implemented for 2 and 3 ' + 'dimensions') @property def is_moving(self) -> bool: @@ -454,6 +457,8 @@ def _get_rotation_matrix(vel: StateVector) -> np.ndarray: theta *= -1 return np.array([[cos(theta), -sin(theta)], [sin(theta), cos(theta)]]) + else: + raise NotImplementedError def _get_angle(vec: StateVector, axis: np.ndarray) -> float: diff --git a/stonesoup/platform/tests/test_platform_base.py b/stonesoup/platform/tests/test_platform_base.py index 7a2972fcb..ca8041d12 100644 --- a/stonesoup/platform/tests/test_platform_base.py +++ b/stonesoup/platform/tests/test_platform_base.py @@ -4,6 +4,7 @@ import numpy as np import pytest +from stonesoup.sensor.sensor import Sensor from stonesoup.types.array import StateVector from ...types.state import State from ...models.transition.linear import ( @@ -103,7 +104,39 @@ def test_base(): assert platform.is_moving -def test_velocity_properties(): +class DummySensor(Sensor): + def measure(self): + pass + + +@pytest.mark.parametrize('class_', [FixedPlatform, MovingPlatform]) +def test_add_sensor(class_): + platform_state = State(StateVector([2, 1, 2, 1, 0, 1]), timestamp=datetime.datetime.now()) + + platform_args = {'transition_model': None} if class_ is MovingPlatform else {} + + sensor = DummySensor() + platform = class_(state=platform_state, position_mapping=[0, 2, 4], + **platform_args) + platform.add_sensor(sensor) + assert (len(platform.mounting_offsets) == 1 + and np.array_equal(platform.mounting_offsets[0], StateVector([0, 0, 0]))) + assert (len(platform.rotation_offsets) == 1 + and np.array_equal(platform.rotation_offsets[0], StateVector([0, 0, 0]))) + + sensor = DummySensor() + platform.add_sensor(sensor, mounting_offset=StateVector([1, 1, 1]), + rotation_offset=StateVector([0, np.pi, np.pi/2])) + assert (len(platform.mounting_offsets) == 2 + and np.array_equal(platform.mounting_offsets[0], StateVector([0, 0, 0])) + and np.array_equal(platform.mounting_offsets[1], StateVector([1, 1, 1]))) + assert (len(platform.rotation_offsets) == 2 + and np.array_equal(platform.rotation_offsets[0], StateVector([0, 0, 0])) + and np.allclose(platform.rotation_offsets[1], StateVector([0, np.pi, np.pi/2]))) + + +@pytest.mark.parametrize('velocity_mapping', [None, [1, 3, 5]]) +def test_velocity_properties(velocity_mapping): model_1d = ConstantVelocity(0.0) model_3d = CombinedLinearGaussianTransitionModel( [model_1d, model_1d, model_1d]) @@ -161,6 +194,66 @@ def test_velocity_properties(): with pytest.raises(AttributeError): _ = platform.velocity + # pass in a velocity mapping + platform_state = State(np.array([[2], + [1], + [2], + [1], + [0], + [1]]), + timestamp) + platform = MovingPlatform(state=platform_state, transition_model=None, + position_mapping=[0, 2, 4], velocity_mapping=velocity_mapping) + assert platform.is_moving + assert np.array_equal(platform.velocity, StateVector([1, 1, 1])) + old_position = platform.position + # check it doesn't move with timestamp = None + platform.move(None) + assert np.array_equal(platform.position, old_position) + + with pytest.raises(AttributeError): + platform.move(timestamp) + + # moving platform without velocity defined + platform_state = State(np.array([[2], + [2], + [0]]), + timestamp) + + platform = MovingPlatform(state=platform_state, transition_model=None, + position_mapping=[0, 2, 4], velocity_mapping=velocity_mapping) + with pytest.raises(AttributeError): + _ = platform.velocity + + +def test_orientation_dimensionality_error(): + platform_state = State(StateVector([2, 1, 1, 1, 2, 0, 1, 0]), + timestamp=datetime.datetime.now()) + + platform = MovingPlatform(state=platform_state, position_mapping=[0, 1, 2, 3], + transition_model=None) + + with pytest.raises(ValueError): + _ = platform.orientation + + platform = MovingPlatform(state=platform_state, position_mapping=[0], transition_model=None) + + with pytest.raises(ValueError): + _ = platform.orientation + + +def test_moving_with_no_initial_timestamp(): + timestamp = datetime.datetime.now() + platform_state = State(StateVector([2, 1, 1, 1, 2, 0]), + timestamp=None) + + platform = MovingPlatform(state=platform_state, position_mapping=[0, 2, 4], + transition_model=None) + + assert platform.timestamp is None + platform.move(timestamp=timestamp) + assert platform.timestamp == timestamp + orientation_tests_3d = [(StateVector([0, 1, 0, 0, 0, 0]), StateVector([0, 0, 0])), (StateVector([0, 0, 0, 0, 0, 1]), StateVector([0, 0, np.pi/2])), diff --git a/stonesoup/sensor/tests/test_sensor.py b/stonesoup/sensor/tests/test_sensor.py index 11d098119..52e15c2bc 100644 --- a/stonesoup/sensor/tests/test_sensor.py +++ b/stonesoup/sensor/tests/test_sensor.py @@ -41,6 +41,16 @@ def test_sensor_position_orientation_setting(): sensor.orientation = StateVector([0, 1, 0]) +def test_warning_on_moving_sensor(): + sensor = DummySensor() + platform_state = State(StateVector([0, 1, 0]), timestamp=datetime.datetime.now()) + platform1 = FixedPlatform(state=platform_state, position_mapping=[0, 1, 2]) + platform2 = FixedPlatform(state=platform_state, position_mapping=[0, 1, 2]) + platform1.add_sensor(sensor) + with pytest.warns(UserWarning): + platform2.add_sensor(sensor) + + def test_default_platform(): sensor = DummySensor(position=StateVector([0, 0, 1])) assert np.array_equal(sensor.position, StateVector([0, 0, 1]))