Sensors: Add delay model

boat_simulator/common/sensors.py

@@ -1,24 +1,44 @@
-from dataclasses import dataclass
-
-from typing import Optional, Any
+from typing import Any
 from numpy.typing import NDArray
+from typing import List
+import numpy as np
 
 
 from boat_simulator.common.types import Scalar, ScalarOrArray
 
 from boat_simulator.common.generators import (
- ConstantGenerator,
  MVGaussianGenerator,
  GaussianGenerator,
 )
 
-WindSensorGenerators = Optional[MVGaussianGenerator | ConstantGenerator]
-GPSGenerators = Optional[GaussianGenerator | ConstantGenerator]
-
 
-@dataclass
 class Sensor:
- """Interface for sensors in the Boat Simulation."""
+ """
+
+ Interface for sensors in the Boat Simulation.
+
+ Data delay and noise models are supported.
+
+ Delay model will delay sensor value updates by one update cycle:
+ - Sensor has initial data x0 at t = 0
+ - Sensor provided new data x_1 at t = 1
+ - Sensor provided new data x_2 at t = 2. At t = 2, x1 is registered into the sensor.
+ - Sensor provided new data x_i at t = i. At t = i, x_{i-1} is registered into the sensor.
+
+ Noise model will add noise to sensor values drawn from a
+ Gaussian or Multi-variate Gaussian distribution.
+ """
+
+ def __init__(self, enable_delay: bool = False, enable_noise: bool = False) -> None:
+ """
+
+ Args:
+ enable_noise (bool): Enables noise for fields. False by default.
+ enable_delay (bool): Enables delay for fields. False by default.
+ """
+
+ self.enable_delay = enable_delay
+ self.enable_noise = enable_noise
 
  def update(self, **kwargs):
  """
@@ -29,6 +49,7 @@ def update(self, **kwargs):
  Raises:
  ValueError: If kwarg is not a defined attribute in Sensor
  """
+
  for attr_name, attr_val in kwargs.items():
  if attr_name in self.__annotations__:
  setattr(self, attr_name, attr_val)
@@ -60,98 +81,179 @@ def read(self, key: str) -> Any:
  )
 
 
-@dataclass
 class WindSensor(Sensor):
  """
  Abstraction for wind sensor.
 
- # TODO: Add delay functions.
-
  Properties:
  wind (ScalarOrArray): Wind x, y components or single value
- wind_noisemaker (Optional[MVGaussianGenerator | ConstantGenerator]):
- Noise function to emulate sensor noise in wind data reading
+ enable_noise (bool): Enables noise for fields. False by default.
+ enable_delay (bool): Enables delay for fields. False by default.
  """
 
  wind: ScalarOrArray
- wind_noisemaker: WindSensorGenerators = None
+
+ def __init__(
+ self,
+ wind: ScalarOrArray,
+ stdev: List[Scalar] = [1.0, 1.0],
+ enable_noise: bool = False,
+ enable_delay: bool = False,
+ ) -> None:
+ super().__init__(enable_noise=enable_noise, enable_delay=enable_delay)
+ self._wind = wind
+
+ # TODO: Refactor the initialization of data fields and their respective delay controls.
+ # Warning: this is not easy!
+
+ self.wind_queue_next: bool = False
+ self.wind_next_value: ScalarOrArray = wind
+ self.wind_noisemaker: MVGaussianGenerator = MVGaussianGenerator(
+ mean=np.array([0, 0]), cov=np.diag(np.power(stdev, 2))
+ )
 
  @property # type: ignore
  def wind(self) -> ScalarOrArray:
  # TODO: Ensure attribute value and noisemakers are using the same value shape.
  # - wind scalars should add with noise scalars.
  # - wind vectors should add with noise vectors.
- # Could consider using a __post_init__ function for this
+
  return (
  self._wind + self.wind_noisemaker.next() # type: ignore
- if self.wind_noisemaker is not None
+ if self.enable_noise
  else self._wind
  )
 
  @wind.setter
  def wind(self, wind: ScalarOrArray):
- self._wind = wind
+
+ if not self.enable_delay:
+ self._wind = wind
+ return
+
+ if self.wind_queue_next:
+ self._wind = self.wind_next_value
+ else:
+ self.wind_queue_next = True
+
+ self.wind_next_value = wind
 
 
-@dataclass
 class GPS(Sensor):
  """
  Abstraction for GPS.
 
- # TODO: Add delay functions.
-
  Properties:
  lat_lon (NDArray): Boat latitude and longitude (2x1 array)
  speed (Scalar): Boat speed
  heading (Scalar): Boat heading
- lat_lon_noisemaker (Optional[GaussianGenerator | ConstantGenerator]):
- Noise function to emulate sensor noise in latitude and longitude readings
- speed_noisemaker (Optional[GaussianGenerator | ConstantGenerator]):
- Noise function to emulate sensor noise in speed readings
- heading_noisemaker (Optional[GaussianGenerator | ConstantGenerator]):
- Noise function to emulate sensor noise in heading readings
+ enable_noise (bool): Enables noise for fields. False by default.
+ enable_delay (bool): Enables delay for fields. False by default.
  """
 
  lat_lon: NDArray
  speed: Scalar
  heading: Scalar
 
- lat_lon_noisemaker: GPSGenerators = None
- speed_noisemaker: GPSGenerators = None
- heading_noisemaker: GPSGenerators = None
+ def __init__(
+ self,
+ lat_lon: NDArray,
+ speed: Scalar,
+ heading: Scalar,
+ stdev: Scalar = 1,
+ enable_noise: bool = False,
+ enable_delay: bool = False,
+ ):
+ super().__init__(enable_noise=enable_noise, enable_delay=enable_delay)
+ self._lat_lon = lat_lon
+ self._speed = speed
+ self._heading = heading
+
+ # TODO: Refactor the initialization of data fields and their respective delay controls.
+ # Warning: this is not easy!
+
+ # Delay Controls
+ self.lat_lon_queue_next: bool = False
+ self.lat_lon_next_value: NDArray = lat_lon
+
+ self.speed_queue_next: bool = False
+ self.speed_next_value: Scalar = speed
+
+ self.heading_queue_next: bool = False
+ self.heading_next_value: Scalar = heading
+
+ self.lat_lon_noisemaker: GaussianGenerator = GaussianGenerator(
+ mean=0, stdev=stdev
+ )
+ self.speed_noisemaker: GaussianGenerator = GaussianGenerator(
+ mean=0, stdev=stdev
+ )
+ self.heading_noisemaker: GaussianGenerator = GaussianGenerator(
+ mean=0, stdev=stdev
+ )
 
  @property # type: ignore
  def lat_lon(self) -> NDArray:
  return (
  self._lat_lon + self.lat_lon_noisemaker.next()
- if self.lat_lon_noisemaker is not None
+ if self.enable_noise
  else self._lat_lon
  )
 
  @lat_lon.setter
  def lat_lon(self, lat_lon: NDArray):
- self._lat_lon = lat_lon
+
+ if not self.enable_delay:
+ self._lat_lon = lat_lon
+ return
+
+ if self.lat_lon_queue_next:
+ self._lat_lon = self.lat_lon_next_value
+ else:
+ self.lat_lon_queue_next = True
+
+ self.lat_lon_next_value = lat_lon
 
  @property # type: ignore
  def speed(self) -> Scalar:
  return (
  self._speed + self.speed_noisemaker.next() # type: ignore
- if self.speed_noisemaker is not None
+ if self.enable_noise
  else self._speed
  )
 
  @speed.setter
  def speed(self, speed: Scalar):
- self._speed = speed
+
+ if not self.enable_delay:
+ self._speed = speed
+ return
+
+ if self.speed_queue_next:
+ self._speed = self.speed_next_value
+ else:
+ self.speed_queue_next = True
+
+ self.speed_next_value = speed
 
  @property # type: ignore
  def heading(self) -> Scalar:
  return (
  self._heading + self.heading_noisemaker.next() # type: ignore
- if self.heading_noisemaker is not None
+ if self.enable_noise
  else self._heading
  )
 
  @heading.setter
  def heading(self, heading: Scalar):
- self._heading = heading
+
+ if not self.enable_delay:
+ self._heading = heading
+ return
+
+ if self.heading_queue_next:
+ self._heading = self.heading_next_value
+ else:
+ self.heading_queue_next = True
+
+ self.heading_next_value = heading

boat_simulator/common/unit_conversions.py

@@ -131,6 +131,9 @@ class ConversionFactors(Enum):
  km_to_nautical_mi = nautical_mi_to_km.inverse()
 
  # Time
+ sec_to_ms = ConversionFactor(factor=1000)
+ ms_to_sec = sec_to_ms.inverse()
+
  min_to_sec = ConversionFactor(factor=60)
  sec_to_min = min_to_sec.inverse()
 
@@ -199,7 +202,9 @@ def __init__(self, **kwargs: EnumAttr):
  belonging to `ConversionFactors`.
  """
  for attr_name, attr_val in kwargs.items():
- assert isinstance(attr_val, Enum) and isinstance(attr_val.value, ConversionFactor)
+ assert isinstance(attr_val, Enum) and isinstance(
+ attr_val.value, ConversionFactor
+ )
  setattr(self, attr_name, attr_val)
 
  def convert(self, **kwargs: ScalarOrArray) -> Dict[str, ScalarOrArray]:
@@ -223,7 +228,9 @@ def convert(self, **kwargs: ScalarOrArray) -> Dict[str, ScalarOrArray]:
 
  for attr_name, attr_val in kwargs.items():
  attr = getattr(self, attr_name, None)
- assert attr is not None, f"Attribute name {attr} not found in UnitConverter."
+ assert (
+ attr is not None
+ ), f"Attribute name {attr} not found in UnitConverter."
 
  conversion_factor = attr.value
  converted_values[attr_name] = conversion_factor.forward_convert(attr_val)