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/models/measurement/base.py b/stonesoup/models/measurement/base.py index 65aa09322..bb25f08e6 100644 --- a/stonesoup/models/measurement/base.py +++ b/stonesoup/models/measurement/base.py @@ -1,18 +1,17 @@ # -*- coding: utf-8 -*- -from abc import abstractmethod +from abc import abstractmethod, ABC -import scipy as sp +import numpy as np 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") @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 4a32e9b65..e64abecd3 100644 --- a/stonesoup/platform/base.py +++ b/stonesoup/platform/base.py @@ -1,69 +1,532 @@ # -*- coding: utf-8 -*- +import datetime +import weakref +from abc import abstractmethod, ABC +from functools import lru_cache +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 +if TYPE_CHECKING: # pragma: no cover + 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` + + """ + 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 " + "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"], default=None, + doc="A list of N mounted sensors. Defaults to an empty list") + 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]``") + + # 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.sensors is None: + self.sensors = [] + + 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.""" + return self.state.state_vector + + @property + def timestamp(self): + return self.state.timestamp + + @property + 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: StateVector) -> None: + self._set_position(value) + + @property + 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) -> 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) -> 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) -> bool: + """Return the ``True`` if the platform is moving, ``False`` otherwise. + """ + raise NotImplementedError + + @abstractmethod + 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: 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. -class Platform(Base): - """Platform base class + 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 + + 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=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 + + @property + 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) -> bool: + return False + + def move(self, timestamp: datetime.datetime, **kwargs) -> None: + """For a fixed platform this method has no effect other than to update the timestamp.""" + # TODO Is this a sensible implementation? + # 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") + .. note:: Position and orientation are a read only properties in this class. + """ transition_model = Property( TransitionModel, doc="Transition model") - def move(self, timestamp=None, **kwargs): + @property + 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) -> StateVector: + """Return the orientation of the platform. + + 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 + 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 + + if self.ndim == 3: + _, bearing, elevation = cart2sphere(*velocity.flat) + return StateVector([0, bearing, elevation]) + 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: + """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 is handled in the move logic + return np.any(self.velocity != 0) + + 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 + # 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`. Parameters ---------- 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. """ if self.state.timestamp is None: self.state.timestamp = timestamp - return self + return # Compute time_interval try: time_interval = timestamp - self.state.timestamp except TypeError: # TypeError: (timestamp or prior.timestamp) is None - time_interval = None + return - # Return without moving static platforms if self.transition_model is None: - self.state.timestamp = timestamp - else: - self.state = State( - state_vector=self.transition_model.function( - state=self.state, - noise=True, - timestamp=timestamp, - time_interval=time_interval, - **kwargs), - timestamp=timestamp) + raise AttributeError('Platform without a transition model cannot be moved') - @property - def state_vector(self): - return self.state.state_vector + self.state = State( + state_vector=self.transition_model.function( + state=self.state, + noise=True, + timestamp=timestamp, + time_interval=time_interval, + **kwargs), + timestamp=timestamp) - @property - def timestamp(self): - return self.state.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)]]) + else: + raise NotImplementedError + + +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 7d51b4161..000000000 --- a/stonesoup/platform/simple.py +++ /dev/null @@ -1,261 +0,0 @@ -# -*- coding: utf-8 -*- -from typing import List - -import numpy as np -from math import cos, sin -from scipy.linalg import expm - -from ..base import Property -from ..types.state import StateVector -from ..sensor import Sensor -from ..functions import cart2pol -from .base import Platform - - -class SensorPlatform(Platform): - """A simple Platform 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). - - """ - - sensors = Property(List[Sensor], 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)") - - # 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) - if self.mounting_mappings.max() > len(self.state.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( - "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( - "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 - - 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) - """ - - # 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 _get_rotated_offset(self, i): - """ Determine the sensor mounting offset for the platforms relative - orientation. - - Parameters - ---------- - i : int - Integer reference to the sensor index - - Returns - ------- - np.array - 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] - - rot = _get_rotation_matrix(vel) - return np.transpose(np.dot(rot, self.mounting_offsets[i])[np.newaxis]) - - -def _get_rotation_matrix(vel): - """ 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 : np.arrary - 1xD 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, axis): - """ 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.array - 1xD array denoting platform velocity - axis : np.array - 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): - """ - 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: np.array - platform velocity - - Returns - ------- - np.array - 3x3 rotation matrix - """ - # 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 = _rot_z(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.array - 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..ca8041d12 100644 --- a/stonesoup/platform/tests/test_platform_base.py +++ b/stonesoup/platform/tests/test_platform_base.py @@ -2,11 +2,14 @@ import datetime 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 ( ConstantVelocity, CombinedLinearGaussianTransitionModel) -from ..base import Platform +from ..base import MovingPlatform, FixedPlatform def test_base(): @@ -19,7 +22,7 @@ def test_base(): platform_state2d = State(np.array([[2], [2]]), timestamp) - platform = Platform(platform_state2d, None) + platform = FixedPlatform(state=platform_state2d, position_mapping=np.array([0, 1])) platform.move(new_timestamp) new_statevector = np.array([[2], [2]]) @@ -27,19 +30,25 @@ 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 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], [2], [2]]), timestamp) - platform = Platform(platform_state3d, None) + platform = FixedPlatform(state=platform_state3d, position_mapping=[0, 1, 2]) platform.move(new_timestamp) new_statevector = np.array([[2], [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 platform.ndim == 3 + assert not platform.is_moving # Define zero noise 2d constant velocity transition model model_1d = ConstantVelocity(0.0) @@ -52,7 +61,8 @@ def test_base(): [2], [1]]), timestamp) - platform = Platform(platform_state2d, model_2d) + platform = MovingPlatform(state=platform_state2d, transition_model=model_2d, + position_mapping=[0, 2]) platform.move(new_timestamp) # Define expected platform location after movement @@ -61,6 +71,9 @@ def test_base(): [4], [1]]) assert (np.array_equal(platform.state.state_vector, new_statevector)) + assert np.array_equal(platform.velocity, StateVector([1, 1])) + assert platform.ndim == 2 + assert platform.is_moving # Define zero noise 3d constant velocity transition model model_3d = CombinedLinearGaussianTransitionModel( @@ -74,7 +87,8 @@ def test_base(): [0], [1]]), timestamp) - platform = Platform(platform_state, model_3d) + 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 @@ -85,5 +99,424 @@ 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])) + assert platform.ndim == 3 + assert platform.is_moving + + +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]) + 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, + position_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, + position_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, + position_mapping=[0, 2, 4]) + 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])), + (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, + 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) + 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])), + ] + - # TODO: More assertions +@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) + + platform_state = State(state, timestamp) + 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) + 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, + position_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, + position_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, position_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) + 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]]), + timestamp) + 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])) + assert np.array_equal(platform.state_vector, StateVector([0, 0, 0])) + + 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=model_3d, + position_mapping=[0, 2, 4]) + with pytest.raises(AttributeError): + platform.position = [0, 0, 0] + + platform_state = State(np.array([[2], + [1], + [2], + [-1], + [2], + [0]]), + timestamp) + 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])) + assert np.array_equal(platform.state_vector, StateVector([0, 1, 0, -1, 1, 0])) + + 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(): + timestamp = datetime.datetime.now() + platform_state = State(np.array([[2], + [2], + [0]]), + timestamp) + platform = MovingPlatform(state=platform_state, transition_model=None, + position_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, position_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, + position_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, 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])) diff --git a/stonesoup/platform/tests/test_platform_simple.py b/stonesoup/platform/tests/test_platform_simple.py index ec24fe972..73410d5de 100644 --- a/stonesoup/platform/tests/test_platform_simple.py +++ b/stonesoup/platform/tests/test_platform_simple.py @@ -4,16 +4,14 @@ import numpy as np import pytest +from ..tests import test_platform_base from ...types.state import State -from ...platform.simple import SensorPlatform +from ..base import MovingPlatform, FixedPlatform from ...models.transition.linear import ( ConstantVelocity, CombinedLinearGaussianTransitionModel) 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: @@ -143,54 +141,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 +181,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 +227,31 @@ 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=[MovingPlatform, FixedPlatform], + ids=['MovingPlatform', 'FixedPlatform']) +def platform_type(request): + return request.param @pytest.fixture(params=[True, False], ids=["Moving", "Static"]) @@ -305,17 +259,22 @@ def move(request): return request.param +@pytest.fixture(params=[True, False], ids=["Add", "Initialise"]) +def add_sensor(request): + return request.param + + 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 = [ @@ -354,7 +313,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): # Define time related variables timestamp = datetime.datetime.now() # Define transition model and position for platform @@ -363,16 +323,27 @@ def test_2d_platform(state, expected, move, radars_2d, mounting_offsets_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) - mounting_mappings = np.array([[0, 2]]) + # 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 = SensorPlatform( - state=platform_state, - transition_model=trans_model, - sensors=radars_2d, - mounting_offsets=mounting_offsets_2d, - mounting_mappings=mounting_mappings - ) + if add_sensor: + platform = MovingPlatform( + state=platform_state, + transition_model=trans_model, + sensors=[], + mounting_offsets=[], + position_mapping=mounting_mapping + ) + for sensor, offset in zip(radars_2d, mounting_offsets_2d): + platform.add_sensor(sensor, offset) + else: + platform = MovingPlatform( + state=platform_state, + transition_model=trans_model, + sensors=radars_2d, + mounting_offsets=mounting_offsets_2d, + position_mapping=mounting_mapping + ) if move: # Move the platform platform.move(timestamp + datetime.timedelta(seconds=2)) @@ -380,28 +351,28 @@ def test_2d_platform(state, expected, move, radars_2d, mounting_offsets_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)) ] @@ -411,7 +382,8 @@ def test_2d_platform(state, expected, move, radars_2d, mounting_offsets_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): # Define time related variables timestamp = datetime.datetime.now() # Define transition model and position for platform @@ -420,20 +392,259 @@ 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) - mounting_mappings = np.array([[0, 2, 4]]) + # 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 = SensorPlatform( + if add_sensor: + platform = MovingPlatform( + state=platform_state, + transition_model=trans_model, + sensors=[], + mounting_offsets=[], + position_mapping=mounting_mapping + ) + for sensor, offset in zip(radars_3d, mounting_offsets_3d): + platform.add_sensor(sensor, offset) + else: + platform = MovingPlatform( + state=platform_state, + transition_model=trans_model, + sensors=radars_3d, + mounting_offsets=mounting_offsets_3d, + position_mapping=mounting_mapping + ) + if move: + # Move the platform + platform.move(timestamp + datetime.timedelta(seconds=2)) + 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]]), + ] + + +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 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 = MovingPlatform( + state=platform_state, + transition_model=trans_model, + sensors=radars_2d, + rotation_offsets=rotation_offsets_2d, + position_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())) +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 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 = MovingPlatform( state=platform_state, transition_model=trans_model, sensors=radars_3d, - mounting_offsets=mounting_offsets_3d, - mounting_mappings=mounting_mappings + rotation_offsets=rotation_offsets_3d, + position_mapping=mounting_mapping ) if move: # Move the platform platform.move(timestamp + datetime.timedelta(seconds=2)) - sensor_positions_test(expected, platform) + assert np.allclose(platform.orientation, expected_platform_orientation) + assert np.allclose(all_sensor_orientations(platform), expected_sensor_orientations) + + +def all_sensor_orientations(platform): + 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): + platform_state = State(state_vector=StateVector([0, 1, 2, 1, 4, 1]), + timestamp=datetime.datetime.now()) + platform_args = {} + if platform_type is MovingPlatform: + platform_args['transition_model'] = None + + if add_sensor: + 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, + 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])) + 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_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 MovingPlatform: + 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, position_mapping=[0, 2, 4], + mounting_offsets=offsets, **platform_args) + + with pytest.raises(ValueError): + _ = platform_type(state=platform_state, sensors=radars_3d, position_mapping=[0, 2, 4], + 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 MovingPlatform: + 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): @@ -446,15 +657,8 @@ def sensor_positions_test(expected_offset, platform): :param platform: platform object :return: """ - radar_position = np.zeros( - [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() - - platform_position = platform.state.state_vector[ - platform.mounting_mappings[i, :]].flatten() + radar_position = all_sensor_positions(platform) + platform_position = platform.position + expected_radar_position = expected_offset + platform_position.T - expected_radar_position[i, :] = (expected_offset[i, :] + - platform_position) assert np.allclose(expected_radar_position, radar_position) 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 3f4027567..2fc8514a7 100644 --- a/stonesoup/sensor/base.py +++ b/stonesoup/sensor/base.py @@ -1,43 +1,105 @@ # -*- coding: utf-8 -*- -from abc import abstractmethod +import weakref +from abc import abstractmethod, ABC +from typing import Optional +from warnings import warn -from ..base import Base, Property -from ..types.state import StateVector +from ..types.array import StateVector +from ..platform import Platform +from ..base import Base -class Sensor(Base): + +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. + """ - @abstractmethod - def measure(self, **kwargs): - """Generate a measurement""" - raise NotImplementedError + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self._platform_system = None + @property + 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.""" + if self.platform_system is None: + return None + else: + return self.platform_system() -class Sensor3DCartesian(Sensor): - """Sensor base class extended to include 3D cartesian motion + @property + def platform_system(self) -> Optional[weakref.ref]: + """Return a ``weakref`` to the platform on which the sensor is mounted""" + return self._platform_system + @platform_system.setter + def platform_system(self, value: weakref.ref): + # this slightly odd construction is to allow overriding by the Sensor subclass + self._set_platform_system(value) - """ - 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") + 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') + self._platform_system = value @abstractmethod def measure(self, **kwargs): - """Generate a measurement""" raise NotImplementedError + + @property + 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`. + + .. note:: + This property delegates the 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.""" + if self.platform is None: + return None + return self.platform.get_sensor_position(self) + + @position.setter + def position(self, value: StateVector): + 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) -> 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 + direction when viewed by an observer looking along the respective rotation axis, + towards the origin. + + .. 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.""" + if self.platform is None: + return None + return self.platform.get_sensor_orientation(self) + + @orientation.setter + def orientation(self, value: StateVector): + 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) -> bool: + return False diff --git a/stonesoup/sensor/passive.py b/stonesoup/sensor/passive.py index e330189fa..183f0ca10 100644 --- a/stonesoup/sensor/passive.py +++ b/stonesoup/sensor/passive.py @@ -1,14 +1,14 @@ # -*- coding: utf-8 -*- import numpy as np -from .base import Sensor3DCartesian +from stonesoup.sensor.sensor 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/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/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 96a289faa..fb4fff380 100644 --- a/stonesoup/sensor/radar/radar.py +++ b/stonesoup/sensor/radar/radar.py @@ -4,9 +4,10 @@ import numpy as np from math import erfc + +from stonesoup.sensor.sensor import Sensor 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 @@ -19,7 +20,7 @@ import scipy.constants as const -class RadarRangeBearing(Sensor3DCartesian): +class RadarRangeBearing(Sensor): """A simple radar sensor that generates measurements of targets, using a :class:`~.CartesianToBearingRange` model, relative to its position. @@ -307,18 +308,14 @@ 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]") + 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]") @@ -373,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) @@ -446,8 +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.translation_offset[self.mapping] # noqa E127 + relative_vector = sky_state.state_vector[self.mapping, :] - self.position relative_vector = self._rotation_matrix @ relative_vector # calculate target position in spherical coordinates @@ -492,7 +493,7 @@ def measure(self, sky_state, noise=True, **kwargs): # Is the state detected? if np.random.rand() <= det_prob: measurement_model = copy.deepcopy(self.measurement_model) - measurement_model.translation_offset = self.translation_offset.copy() + measurement_model.translation_offset = self.position.copy() measurement_model.rotation_offset = self.rotation_offset.copy() 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..78d112be4 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 @@ -22,25 +22,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_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 + theta_z = -rotation_offset[2, 0] + theta_y = -rotation_offset[1, 0] + theta_x = -rotation_offset[0, 0] + + rotation_matrix = rotz(theta_z)@roty(theta_y)@rotx(theta_x) xyz_rot = rotation_matrix @ xyz x = xyz_rot[0, 0] @@ -58,31 +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]) # Create a radar object - radar = RadarRangeBearing( - position=radar_position, - orientation=radar_orientation, - ndim_state=2, - mapping=measurement_mapping, - noise_covar=noise_covar) + 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) @@ -116,16 +97,15 @@ def test_rotating_radar(): measurement_mapping = np.array([0, 1]) # Create a radar object - 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) + 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()) @@ -166,8 +146,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 +154,20 @@ 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(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) # Assert that the object has been correctly initialised assert np.array_equal(radar.position, radar_position) @@ -234,7 +210,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 +256,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 +273,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 +305,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, @@ -338,10 +319,9 @@ 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)) + measurement_model=LinearGaussian(noise_covar=np.diag([1, 1, 1]), + mapping=[0, 2, 4], + ndim_state=6)) assert radar.measure(target) is None @@ -353,25 +333,25 @@ 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], - translation_offset=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], + 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.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 diff --git a/stonesoup/sensor/sensor.py b/stonesoup/sensor/sensor.py new file mode 100644 index 000000000..976167850 --- /dev/null +++ b/stonesoup/sensor/sensor.py @@ -0,0 +1,49 @@ +from abc import ABC + +from ..platform.base import FixedPlatform +from .base import BaseSensor +from ..types.array import StateVector +from ..types.state import State + + +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 FixedPlatform 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]) + if orientation is None: + orientation = StateVector([0, 0, 0]) + self._internal_platform = FixedPlatform( + state=State(state_vector=position), + 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') + super()._set_platform_system(value) diff --git a/stonesoup/sensor/tests/test_sensor.py b/stonesoup/sensor/tests/test_sensor.py new file mode 100644 index 000000000..52e15c2bc --- /dev/null +++ b/stonesoup/sensor/tests/test_sensor.py @@ -0,0 +1,88 @@ +import datetime + +import pytest + +from ...platform.base import FixedPlatform +from ..base import BaseSensor +from ..sensor import Sensor +from ...types.array import StateVector +from ...types.state import State + +import numpy as np + + +class DummySensor(Sensor): + def measure(self, **kwargs): + pass + + +class DummyBaseSensor(BaseSensor): + def measure(self, **kwargs): + pass + + +def test_sensor_position_orientation_setting(): + 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]) + 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 = 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) + with pytest.raises(AttributeError): + sensor.position = StateVector([0, 1, 0]) + with pytest.raises(AttributeError): + 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])) + assert np.array_equal(sensor.orientation, StateVector([0, 0, 0])) + + 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 = DummySensor(position=StateVector([0, 0, 1])) + assert sensor._has_internal_platform + + sensor = DummySensor() + assert not sensor._has_internal_platform + + sensor = DummyBaseSensor() + assert not sensor._has_internal_platform + + +def test_changing_platform_from_default(): + 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]) + with pytest.raises(AttributeError): + platform.add_sensor(sensor) + + +@pytest.mark.parametrize('sensor', [DummyBaseSensor, DummySensor]) +def test_sensor_measure(sensor): + # needed for test coverage... Does no harm + assert sensor().measure() is None diff --git a/stonesoup/serialise.py b/stonesoup/serialise.py index 8d80cfc71..08b859af5 100644 --- a/stonesoup/serialise.py +++ b/stonesoup/serialise.py @@ -18,7 +18,9 @@ import ruamel.yaml from ruamel.yaml.constructor import ConstructorError -from .base import Base +from .types.array import StateVector +from .sensor.sensor import Sensor +from .base import Base, Property class YAML: @@ -90,10 +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: + 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/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..86950fd6d 100644 --- a/stonesoup/simulator/tests/test_platform.py +++ b/stonesoup/simulator/tests/test_platform.py @@ -1,12 +1,12 @@ # -*- coding: utf-8 -*- import datetime +from copy import deepcopy import numpy as np -from ...models.transition.linear import \ - CombinedLinearGaussianTransitionModel, ConstantVelocity -from ...platform.simple import SensorPlatform +from stonesoup.platform.base import MovingPlatform +from ...models.transition.linear import CombinedLinearGaussianTransitionModel, ConstantVelocity from ...types.state import State from ..platform import PlatformDetectionSimulator from ..simple import SingleTargetGroundTruthSimulator @@ -18,11 +18,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, + position_mapping = np.array([[0, 2]]) + platform = MovingPlatform(state=state, sensors=sensors, transition_model=trans_model, mounting_offsets=mounting_offsets, - mounting_mappings=mounting_mappings) + position_mapping=position_mapping) return platform @@ -92,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)) 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) 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):