Updated sensors

spatz/sensors/imu/accelerometer.py

@@ -29,7 +29,6 @@ class Accelerometer(Sensor):
     def __init__(
             self, dataset: DataSource, 
             logger: Logger, 
-            coord_system: CoordSystem = CoordSystem.RIGHT_HANDED,
             orientation: NDArray = np.identity(3), 
             offset: float = 0, 
             transforms: List[Transform] = []):
@@ -48,27 +47,22 @@ class Accelerometer(Sensor):
         assert orientation.shape == (3, 3), 'Orientation has to be a 3x3 matrix.'
 
         self._offset = np.array([offset, 0, 0])
-        self._coord_system = coord_system
         self._orientation = orientation
 
     def _get_data(self) -> ArrayLike | float:
         acc = self._dataset.get_acceleration('global')
 
-        # self._logger.write('global_ax', acc[0], self._get_name())
-        # self._logger.write('global_ay', acc[1], self._get_name())
-        # self._logger.write('global_az', acc[2], self._get_name())
+        self._logger.write('global_ax', acc[0], self._get_name())
+        self._logger.write('global_ay', acc[1], self._get_name())
+        self._logger.write('global_az', acc[2], self._get_name())
 
         # Convert FL to body
         acc = self._dataset.global_to_local() @ acc
-        # acc += mat @ np.array([0, 0, g])
+        acc += self._dataset.global_to_local() @ np.array([0, 0, g])
 
-        # self._logger.write('local_x', acc[0], self._get_name())
-        # self._logger.write('local_y', acc[1], self._get_name())
-        # self._logger.write('local_z', acc[2], self._get_name())
-
-        # Flip axes to sensor's perspective. Spatz uses a right-handed coordinate system.
-        #if self._coord_system == CoordSystem.LEFT_HANDED:
-        #    acc[1] *= -1
+        self._logger.write('local_x', acc[0], self._get_name())
+        self._logger.write('local_y', acc[1], self._get_name())
+        self._logger.write('local_z', acc[2], self._get_name())
 
         # Rotate the acceleration vector to accomodate the accelerometer's orientation on the rocket.
         acc = self._orientation @ acc

spatz/sensors/imu/gyroscope.py

@@ -1,21 +1,30 @@
+import numpy as np
+
 from numpy.typing import ArrayLike
 from typing import List
 
 from spatz.sensors import Sensor
 from spatz.transforms import Transform
-from spatz.dataset import Dataset
 from spatz.simulations.data_source import DataSource
 from spatz.logger import Logger
 
 
 class Gyroscope(Sensor):
-    def __init__(self, dataset: DataSource, logger: Logger, offset: float = 0, transforms: List[Transform] = []):
+    def __init__(self, dataset: DataSource, logger: Logger, orientation=np.identity(3), transforms: List[Transform] = []):
         super().__init__(dataset, logger, transforms)
 
-        self._offset = offset
+        self._orientation = orientation
+
+    def calibrate(self, n_samples):
+        return np.sum(self() for i in range(n_samples)) / n_samples
 
     def _get_data(self) -> ArrayLike | float:
         # Rotation in rad/sec
-        x = self._dataset.get_angular_velocity()
+        omegas = self._dataset.get_angular_velocity()
+        omegas = self._orientation @ omegas
 
-        return x
+        self._log('ox', omegas[0])
+        self._log('oy', omegas[1])
+        self._log('oz', omegas[2])
+
+        return omegas

spatz/sensors/imu/h3lis100dl.py

@@ -13,7 +13,7 @@ from spatz.transforms import Transform
 
 class H3LIS100DL(Accelerometer):
     def __init__(self, dataset: DataSource, logger: Logger, orientation=np.identity(3), offset: float = 0, transforms: List[Transform] = []):        
-        super().__init__(dataset, logger, CoordSystem.RIGHT_HANDED, orientation, offset, transforms)
+        super().__init__(dataset, logger, orientation, offset, transforms)
 
     def _get_name(self) -> AnyStr:
         return "H3LIS100DL"
@@ -23,6 +23,14 @@ class H3LIS100DL(Accelerometer):
         g = 9.81
         x = np.floor(np.clip(x, -100*g, +100*g) / g)
 
+        for i in range(3):
+            value = np.random.random()
+            if (value < 0.1):
+                x[i] += 1
+
+            if (value > 0.9):
+                x[i] -= 1
+
         return x
 
 

spatz/sensors/imu/wsen_isds.py

@@ -4,7 +4,7 @@ from typing import AnyStr, List
 from numpy.typing import ArrayLike
 
 from spatz.sensors import Accelerometer, Gyroscope, IMU, CoordSystem
-from spatz.transforms import Transform, GaussianNoise
+from spatz.transforms import Transform, GaussianNoise, DriftingBias
 from spatz.simulations.data_source import DataSource
 from spatz.dataset import Dataset
 from spatz.logger import Logger
@@ -14,26 +14,37 @@ class WSEN_ISDS(IMU):
     pass
 
 
+g = 9.81
+
+
 class WSEN_ISDS_ACC(Accelerometer):
     def __init__(self, dataset: DataSource, logger: Logger, orientation=np.identity(3), offset=0, transforms: List[Transform] = []):
-        super().__init__(dataset, logger, CoordSystem.LEFT_HANDED, orientation, offset, transforms)
+        super().__init__(dataset, logger, orientation, offset, transforms)
 
-        self.__variance = 0.05
-        self.__noise = GaussianNoise(np.zeros(3), np.identity(3) * self.__variance)
+        self.__bias = DriftingBias(np.zeros(3), np.array([
+            0.00113044 / g * 1000, 
+            0.00108539 / g * 1000, 
+            0.00127884 / g * 1000
+        ]), 400)
+
+        self.__constant_bias = np.random.normal(0, 0.81423, 3)
+
+        self.__normal = GaussianNoise(0, np.array([
+            0.0003330315865455515 / g * 1000, 
+            0.00016874534484267122 / g * 1000, 
+            0.0003885568325537318 / g * 100
+        ]))
 
     def _get_name(self) -> AnyStr:
         return 'WSEN_ISDS_ACC'
 
     def _sensor_specific_effects(self, x: ArrayLike) -> ArrayLike:
         t = self._dataset.get_time()
-
-        g = 9.81
-
         # Convert to milli-g.
         x = x / g * 1000
 
         # Apply noise to the true values.
-        y = self.__noise(t, x)
+        y = self.__constant_bias + self.__normal(t, self.__bias(t, x))
         noise = y - x
 
         # Log the chosen noise values.
@@ -48,8 +59,12 @@ class WSEN_ISDS_ACC(Accelerometer):
     
 
 class WSEN_ISDS_GYRO(Gyroscope):
-    def __init__(self, dataset: Dataset, logger: Logger, offset=0, transforms: List[Transform] = []):
-        super().__init__(dataset, logger, offset, transforms)
+    def __init__(self, dataset: Dataset, logger: Logger, orientation = np.identity(3), transforms: List[Transform] = []):
+        super().__init__(dataset, logger, orientation, transforms)
+
+        self.__bias = DriftingBias(np.zeros(3), np.array([0.00218 * 1000, 0.00105 * 1000, 0.00203 * 1000]), 400)
+        self.__constant_bias = np.random.normal(0, 2*2000, 3)
+        self.__normal = GaussianNoise(0, np.array([0.0049272 * 1000, 0.00557833 * 1000, 0.00407826 * 1000]))
 
     def _get_name(self) -> AnyStr:
         return 'WSEN_ISDS_GYRO'
@@ -58,6 +73,9 @@ class WSEN_ISDS_GYRO(Gyroscope):
         # Convert to milli-degrees per second.
         x = (x / np.pi) * 180 * 1000
 
+        t = self._dataset.get_time()
+        x = self.__constant_bias + self.__normal(t, self.__bias(t, x))
+
         # TODO: Noise model.
 
         self._log('ox', x[0])

spatz/sensors/pressure/ms5611.py

@@ -15,7 +15,7 @@ class MS5611(PressureSensor):
         # Noise model obtained by a test flight using this sensor.
         # self.__pad_noise = GaussianNoise(0, 0.03)
         # self.__flight_noise = GaussianNoise(0, 1.5)
-        self.__noise = ProportionalGaussian(0, 0.0015)
+        self.__noise = GaussianNoise(0, 0.00043300242654881085)
 
     def _get_name(self) -> AnyStr:
         return 'MS5611'