class TestSMS(StateMutableSequence):
test_property: int = Property(default=3)
def __init__(self, *args, **kwargs):
super().__init__(*args, **kwargs)
self.test_variable = 5
def test_method(self):
pass
@property
def complicated_attribute(self):
if self.test_property == 3:
return self.test_property
else:
raise AttributeError('Custom error message')
class Detector(DetectionReader):
trajectory: Trajectory = Property(doc="")
@BufferedGenerator.generator_method
def detections_gen(self):
detections_df = traj.df.loc[:, "geometry"].to_frame()
if traj.is_latlon:
detections_df.to_crs("EPSG:3395", inplace=True)
detections_df["x"] = [
row.geometry.coords[0][0] for _, row in detections_df.iterrows()
]
detections_df["y"] = [
row.geometry.coords[0][1] for _, row in detections_df.iterrows()
]
detections_df.drop(columns="geometry", inplace=True)
for time, row in detections_df.iterrows():
detection = Detection([row.x, row.y], timestamp=time)
yield time, {detection}
class FixedMovable(Movable):
"""Fixed platform base class
A platform represents a random object defined as a :class:`~.StateMutableSequence`
with fixed (but settable) position and orientation.
.. note:: Position and orientation are read/write properties in this class.
"""
orientation: StateVector = Property(
default=None,
doc=
'A fixed orientation of the static platform. Defaults to the zero vector'
)
def __init__(self, *args, **kwargs):
velocity_mapping = kwargs.get('velocity_mapping', None)
if velocity_mapping:
raise ValueError(
'Velocity mapping should not be set for a FixedMovable')
super().__init__(*args, **kwargs)
self.velocity_mapping = None
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."""
new_state = copy.deepcopy(self.state)
new_state.timestamp = timestamp
self.states.append(new_state)
class Platform(Base):
"""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).
Movement is controlled by the Platform's :attr:`Platform.movement_controller`, and access to
attributes of the Platform is proxied to the movement controller, to allow the Platform to
report it's own position, orientation etc.
If a ``movement_controller`` argument is not supplied to the constructor, the Platform will
try to construct one using unused arguments passed to the Platform's constructor.
.. note:: This class is 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`
"""
movement_controller: Movable = Property(
default=None,
doc=
":class:`~.Movable` object to control the Platform's movement. Default is None, but "
"it can be constructed transparently by passing Movable's constructor parameters to "
"the Platform constructor.")
sensors: MutableSequence[Sensor] = Property(
default=None,
readonly=True,
doc="A list of N mounted sensors. Defaults to an empty list")
_default_movable_class = None # Will be overridden by subclasses
def __getattribute__(self, name):
# This method is called if we try to access an attribute of self. First we try to get the
# attribute directly, but if that fails, we want to try getting the same attribute from
# self.movement_controller instead. If that, in turn, fails we want to return the error
# message that would have originally been raised, rather than an error message that the
# Movable has no such attribute.
#
# An alternative mechanism using __getattr__ seems simpler (as it skips the first few lines
# of code) but __getattr__ has no mechanism to capture the originally raised error.
try:
# This tries first to get the attribute from self.
return Base.__getattribute__(self, name)
except AttributeError as original_error:
if name.startswith("_"):
# Don't proxy special/private attributes to `movement_controller`, just raise the
# original error
raise original_error
else:
# For non _ attributes, try to get the attribute from self.movement_controller
# instead of self.
try:
controller = Base.__getattribute__(self,
'movement_controller')
return getattr(controller, name)
except AttributeError:
# If we get the error about 'Movable' not having the attribute, then we want to
# raise the original error instead
raise original_error
def __init__(self, *args, **kwargs):
platform_arg_names = self._properties.keys()
platform_args = {
key: value
for key, value in kwargs.items() if key in platform_arg_names
}
other_args = {
key: value
for key, value in kwargs.items() if key not in platform_arg_names
}
super().__init__(**platform_args)
if self.movement_controller is None:
self.movement_controller = self._default_movable_class(
*args, **other_args)
if self.sensors is None:
self._property_sensors = []
for sensor in self.sensors:
sensor.movement_controller = self.movement_controller
@staticmethod
def _tuple_or_none(value):
return None if value is None else tuple(value)
@sensors.getter
def sensors(self):
return self._tuple_or_none(self._property_sensors)
def add_sensor(self, sensor: Sensor) -> None:
""" Add a sensor to the platform
Parameters
----------
sensor : :class:`~.BaseSensor`
The sensor object to add
"""
self._property_sensors.append(sensor)
sensor.movement_controller = self.movement_controller
def remove_sensor(self, sensor: Sensor) -> None:
""" Remove a sensor from the platform
Parameters
----------
sensor : :class:`~.BaseSensor`
The sensor object to remove
"""
self.pop_sensor(self._property_sensors.index(sensor))
def pop_sensor(self, index: int = -1):
"""
Remove and return a sensor from the platform by index. If no index is specified, remove
and return the last sensor in :attr:`self.sensors`
Parameters
----------
index : int
The index of the sensor to remove. Defaults to the last item in the list.
"""
return self._property_sensors.pop(index)
# The position, orientation and velocity getters are not required, as __getattribute__ will do
# the job, but the setters are required, and this seems the cleanest way to implement them
@property
def position(self):
return self.movement_controller.position
@position.setter
def position(self, value):
self.movement_controller.position = value
@property
def velocity(self):
return self.movement_controller.velocity
@velocity.setter
def velocity(self, value):
self.movement_controller.velocity = value
@property
def orientation(self):
return self.movement_controller.orientation
@orientation.setter
def orientation(self, value):
self.movement_controller.orientation = value
def __getitem__(self, item):
return self.movement_controller.__getitem__(item)
class rjmcmc(Base, BufferedGenerator):
csv_path: str = Property(
doc='The path to the csv file, containing the raw data')
@BufferedGenerator.generator_method
def detections_gen(self):
detections = set()
current_time = datetime.now()
num_samps = 1000000
d = 10
omega = 50
fs = 20000
l = 1 # expected number of targets
window = 20000
windowm1 = window - 1
y = np.loadtxt(self.csv_path, delimiter=',')
L = len(y)
N = 9 * window
max_targets = 5
nbins = 128
bin_steps = [(math.pi + 0.1) / (2 * nbins), 2 * math.pi / nbins]
scans = []
winstarts = np.linspace(0, L - window, num=int(L / window), dtype=int)
for win in winstarts:
# initialise histograms
param_hist = np.zeros([max_targets, nbins, nbins])
order_hist = np.zeros([max_targets])
# initialise params
p_params = np.empty([max_targets, 2])
noise = noise_proposal(0)
[params, K] = proposal([], 0, p_params)
# calculate sinTy and cosTy
sinTy = np.zeros([9])
cosTy = np.zeros([9])
alpha = np.zeros([9])
yTy = 0
for k in range(0, 9):
for t in range(0, window):
sinTy[k] = sinTy[k] + math.sin(
2 * math.pi * t * omega / fs) * y[t + win, k]
cosTy[k] = cosTy[k] + math.cos(
2 * math.pi * t * omega / fs) * y[t + win, k]
yTy = yTy + y[t + win, k] * y[t + win, k]
sumsinsq = 0
sumcossq = 0
sumsincos = 0
for t in range(0, window):
sumsinsq = sumsinsq + math.sin(
2 * math.pi * t * omega / fs) * math.sin(
2 * math.pi * t * omega / fs)
sumcossq = sumcossq + math.cos(
2 * math.pi * t * omega / fs) * math.cos(
2 * math.pi * t * omega / fs)
sumsincos = sumsincos + math.sin(
2 * math.pi * t * omega / fs) * math.cos(
2 * math.pi * t * omega / fs)
old_logp = calc_acceptance(noise, params, K, omega, 1, d, y,
window, sinTy, cosTy, yTy, alpha,
sumsinsq, sumcossq, sumsincos, N, l)
n = 0
while n < num_samps:
p_noise = noise_proposal(noise)
[p_params, p_K,
Qratio] = proposal_func(params, K, p_params, max_targets)
if p_K != 0:
new_logp = calc_acceptance(p_noise, p_params, p_K, omega,
1, d, y, window, sinTy, cosTy,
yTy, alpha, sumsinsq, sumcossq,
sumsincos, N, l)
logA = new_logp - old_logp + np.log(Qratio)
# do a Metropolis-Hastings step
if logA > np.log(random.uniform(0, 1)):
old_logp = new_logp
params = copy.deepcopy(p_params)
K = copy.deepcopy(p_K)
for k in range(0, K):
bin_ind = [0, 0]
for l in range(0, 2):
edge = bin_steps[l]
while edge < params[k, l]:
edge += bin_steps[l]
bin_ind[l] += 1
if bin_ind[l] == nbins - 1:
break
param_hist[K - 1, bin_ind[0], bin_ind[1]] += 1
order_hist[K - 1] += 1
n += 1
# look for peaks in histograms
max_peak = 0
max_ind = 0
for ind in range(0, max_targets):
if order_hist[ind] > max_peak:
max_peak = order_hist[ind]
max_ind = ind
# FOR TESTING PURPOSES ONLY - SET max_ind = 0
max_ind = 0
# look for largest N peaks, where N corresponds to peak in the order histogram
# use divide-and-conquer quadrant-based approach
if max_ind == 0:
[unique_peak_inds1, unique_peak_inds2
] = np.unravel_index(param_hist[0, :, :].argmax(),
param_hist[0, :, :].shape)
num_peaks = 1
else:
order_ind = max_ind - 1
quadrant_factor = 2
nstart = 0
mstart = 0
nend = quadrant_factor
mend = quadrant_factor
peak_inds1 = [None] * 16
peak_inds2 = [None] * 16
k = 0
while quadrant_factor < 32:
max_quadrant = 0
quadrant_size = nbins / quadrant_factor
for n in range(nstart, nend):
for m in range(mstart, mend):
[ind1, ind2] = np.unravel_index(
param_hist[order_ind,
int(n * quadrant_size):int(
(n + 1) * quadrant_size - 1),
int(m * quadrant_size):int(
(m + 1) * quadrant_size -
1)].argmax(),
param_hist[order_ind,
int(n * quadrant_size):int(
(n + 1) * quadrant_size - 1),
int(m * quadrant_size):int(
(m + 1) * quadrant_size -
1)].shape)
peak_inds1[k] = int(ind1 + n * quadrant_size)
peak_inds2[k] = int(ind2 + m * quadrant_size)
if param_hist[order_ind, peak_inds1[k],
peak_inds2[k]] > max_quadrant:
max_quadrant = param_hist[order_ind,
peak_inds1[k],
peak_inds2[k]]
max_ind1 = n
max_ind2 = m
k += 1
quadrant_factor = 2 * quadrant_factor
# on next loop look for other peaks in the quadrant containing the highest peak
nstart = 2 * max_ind1
mstart = 2 * max_ind2
nend = 2 * (max_ind1 + 1)
mend = 2 * (max_ind2 + 1)
# determine unique peaks
unique_peak_inds1 = [None] * 16
unique_peak_inds2 = [None] * 16
unique_peak_inds1[0] = peak_inds1[0]
unique_peak_inds2[0] = peak_inds2[0]
num_peaks = 1
for n in range(0, 16):
flag_unique = 1
for k in range(0, num_peaks):
# check if peak is close to any other known peaks
if (unique_peak_inds1[k] - peak_inds1[n]) < 2:
if (unique_peak_inds2[k] - peak_inds2[n]) < 2:
# part of same peak (check if bin is taller)
if param_hist[order_ind, peak_inds1[n],
peak_inds2[n]] > param_hist[
order_ind,
unique_peak_inds1[k],
unique_peak_inds2[k]]:
unique_peak_inds1 = peak_inds1[n]
unique_peak_inds2 = peak_inds2[n]
flag_unique = 0
break
if flag_unique == 1:
unique_peak_inds1[num_peaks] = peak_inds1[n]
unique_peak_inds2[num_peaks] = peak_inds2[n]
num_peaks += 1
# Defining a detection
state_vector = StateVector([
unique_peak_inds2 * bin_steps[1],
unique_peak_inds1 * bin_steps[0]
]) # [Azimuth, Elevation]
covar = CovarianceMatrix(np.array([[1, 0],
[0, 1]])) # [[AA, AE],[AE, EE]]
measurement_model = LinearGaussian(ndim_state=4,
mapping=[0, 2],
noise_covar=covar)
current_time = current_time + timedelta(milliseconds=window)
detection = Detection(state_vector,
timestamp=current_time,
measurement_model=measurement_model)
detections = set([detection])
scans.append((current_time, detections))
# For every timestep
for scan in scans:
yield scan[0], scan[1]
class capon(Base, BufferedGenerator):
csv_path: str = Property(
doc='The path to the csv file, containing the raw data')
@BufferedGenerator.generator_method
def detections_gen(self):
detections = set()
current_time = datetime.now()
y = np.loadtxt(self.csv_path, delimiter=',')
L = len(y)
# frequency of sinusoidal signal
omega = 50
window = 20000
windowm1 = window - 1
thetavals = np.linspace(0, 2 * math.pi, num=400)
phivals = np.linspace(0, math.pi / 2, num=100)
# spatial locations of hydrophones
z = np.matrix(
'0 0 0; 0 10 0; 0 20 0; 10 0 0; 10 10 0; 10 20 0; 20 0 0; 20 10 0; 20 20 0'
)
N = 9 # No. of hydrophones
# steering vector
v = np.zeros(N, dtype=np.complex)
# directional unit vector
a = np.zeros(3)
scans = []
winstarts = np.linspace(0, L - window, num=int(L / window), dtype=int)
c = 1481 / (2 * omega * math.pi)
for t in winstarts:
# calculate covariance estimate
R = np.matmul(np.transpose(y[t:t + windowm1]), y[t:t + windowm1])
R_inv = np.linalg.inv(R)
maxF = 0
maxtheta = 0
maxfreq = 0
for theta in thetavals:
for phi in phivals:
# convert from spherical polar coordinates to cartesian
a[0] = math.cos(theta) * math.sin(phi)
a[1] = math.sin(theta) * math.sin(phi)
a[2] = math.cos(phi)
a = a / math.sqrt(np.sum(a * a))
for n in range(0, N):
phase = np.sum(a * np.transpose(z[n, ])) / c
v[n] = math.cos(phase) - math.sin(phase) * 1j
F = 1 / (
(window - N) * np.transpose(np.conj(v)) @ R_inv @ v)
if F > maxF:
maxF = F
maxtheta = theta
maxphi = phi
# Defining a detection
state_vector = StateVector([maxtheta,
maxphi]) # [Azimuth, Elevation]
covar = CovarianceMatrix(np.array([[1, 0],
[0, 1]])) # [[AA, AE],[AE, EE]]
measurement_model = LinearGaussian(ndim_state=4,
mapping=[0, 2],
noise_covar=covar)
current_time = current_time + timedelta(milliseconds=window)
detection = Detection(state_vector,
timestamp=current_time,
measurement_model=measurement_model)
detections = set([detection])
scans.append((current_time, detections))
# For every timestep
for scan in scans:
yield scan[0], scan[1]
class MultiTransitionMovable(MovingMovable):
"""Moving platform with multiple transition models
A list of transition models are given with corresponding transition times, dictating the
movement behaviour of the platform for given durations.
"""
transition_models: Sequence[TransitionModel] = Property(
doc="List of transition models")
transition_times: Sequence[datetime.timedelta] = Property(
doc="Durations for each listed "
"transition model")
def __init__(self, *args, **kwargs):
super().__init__(*args, **kwargs)
if len(self.transition_models) != len(self.transition_times):
raise AttributeError(
'transition_models and transition_times must be same length')
self.transition_index = 0
self.current_interval = self.transition_times[0]
@property
def transition_model(self):
return self.transition_models[self.transition_index]
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 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.
This method updates :attr:`transition_model`, :attr:`transition_index` and
:attr:`current_interval`:
If the timestamp provided gives a time delta greater than :attr:`current_interval` the
:attr:`transition_model` is called for the rest of its corresponding duration, and the move
method is called again on the next transition model (by incrementing
:attr:`transition_index`) in :attr:`transition_models` with the residue time delta.
If the time delta is less than :attr:`current_interval` the :attr:`transition_model` is
called for that duration and :attr:`current_interval` is reduced accordingly.
"""
if self.state.timestamp is None:
self.state.timestamp = timestamp
return
try:
time_interval = timestamp - self.state.timestamp
except TypeError:
# TypeError: (timestamp or prior.timestamp) is None
return
temp_state = self.state
while time_interval != 0:
if time_interval >= self.current_interval:
temp_state = State(state_vector=self.transition_model.function(
state=temp_state,
noise=True,
time_interval=self.current_interval,
**kwargs),
timestamp=timestamp)
time_interval -= self.current_interval
self.transition_index = (self.transition_index + 1) % len(
self.transition_models)
self.current_interval = self.transition_times[
self.transition_index]
else:
temp_state = State(state_vector=self.transition_model.function(
state=temp_state,
noise=True,
time_interval=time_interval,
**kwargs),
timestamp=timestamp)
self.current_interval -= time_interval
time_interval = 0
self.states.append(temp_state)
class MovingMovable(Movable):
"""Moving platform base class
A platform represents a random object defined as a :class:`~.State`
that moves according to a given :class:`~.TransitionModel`.
.. note:: Position and orientation are a read only properties in this class.
"""
transition_model: TransitionModel = Property(doc="Transition model")
@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, elevation, bearing])
elif self.ndim == 2:
_, bearing = cart2pol(*velocity.flat)
return StateVector([0, 0, bearing])
else:
raise NotImplementedError(
'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``)
Notes
-----
This methods updates the value of :attr:`position`.
Any provided ``kwargs`` are forwarded to the :attr:`transition_model`.
If `timestamp`` is ``None``, the method has no effect, but will return successfully.
"""
if self.state.timestamp is None:
self.state.timestamp = timestamp
return
# Compute time_interval
try:
time_interval = timestamp - self.state.timestamp
except TypeError:
# TypeError: (timestamp or prior.timestamp) is None
return
if self.transition_model is None:
raise AttributeError(
'Platform without a transition model cannot be moved')
self.states.append(
State(state_vector=self.transition_model.function(
state=self.state,
noise=True,
timestamp=timestamp,
time_interval=time_interval,
**kwargs),
timestamp=timestamp))
class Movable(StateMutableSequence, ABC):
states: Sequence[State] = Property(
doc="A list of States which enables the platform's history to be "
"accessed in simulators and for plotting. Initiated as a "
"state, 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: Sequence[int] = Property(
doc="Mapping between platform position and state vector. For a "
"position-only 3d platform this might be ``[0, 1, 2]``. For a "
"position and velocity platform: ``[0, 2, 4]``")
velocity_mapping: Sequence[int] = Property(
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.velocity_mapping is None:
self.velocity_mapping = [p + 1 for p in self.position_mapping]
if not self.states:
raise ValueError(
'States must not be empty: it must contain least one state.')
@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
@property
@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 _get_rotated_offset(self, offset: StateVector) -> np.ndarray:
""" Determine the sensor mounting offset for the platforms relative
orientation.
Parameters
----------
offset : :class:`~.StateVector`
Mounting offset to rotate
Returns
-------
: :class:`np.ndarray`
Sensor mounting offset rotated relative to platform motion
"""
if self.is_moving:
vel = self.velocity
rot = _get_rotation_matrix(vel)
return rot @ offset
else:
return offset
def range_and_angles_to_other(
self, other: 'Movable') -> Tuple[float, float, float]:
""" Calculate the range, azimuth and elevation of a given Movable relative to current
Movable.
Calculates the relative vector between the two Movables, and then converts this
range, azimuth, elevation using :func:`.cart2sphere`
Parameters
----------
other : :class:`~.Movable`
Another Movable. Only its position is relevant to this method.
Returns
-------
range, azimuth, elevation : :class:`float`, :class:`float`, :class:`float`
The range azimuth and elevation of the target from the radar
"""
# state relative to radar (in cartesian space)
relative_vector = other.position - self.position
relative_vector = self._rotation_matrix @ relative_vector
# calculate target position in spherical coordinates
[range_, azimuth, elevation] = cart2sphere(*relative_vector)
return range_, azimuth, elevation
@property
def _rotation_matrix(self) -> np.ndarray:
"""_rotation_matrix getter method
Calculates and returns the (3D) axis rotation matrix.
Returns
-------
: :class:`~numpy.ndarray` of shape (3, 3)
The model (3D) rotation matrix.
"""
return build_rotation_matrix(self.orientation)