Current state of the simulation

spatz/sensors/imu/bhi160.py

@@ -1,10 +1,12 @@
+import numpy as np
+
 from typing import List, AnyStr
 
 from numpy.typing import ArrayLike
 from spatz.dataset import ArrayLike, Dataset
 from spatz.logger import ArrayLike, Logger
 from spatz.sensors import IMU, Accelerometer, Gyroscope
-from spatz.transforms import Transform
+from spatz.transforms import Transform, GaussianNoise
 
 
 class BHI160Gyro(Gyroscope):
@@ -27,8 +29,10 @@ class BHI160Acc(Accelerometer):
     def __init__(self, dataset: Dataset, logger: Logger, offset: float = 0, transforms: List[Transform] = []):
         super().__init__(dataset, logger, offset, transforms)
 
+        self.__noise = GaussianNoise(0, 0.05)
+
     def _get_name(self) -> AnyStr:
         return 'BHI160'
     
     def _sensor_specific_effects(self, x: ArrayLike) -> ArrayLike:
-        return x
+        return self.__noise(0, x)

spatz/transforms/noise.py

@@ -13,15 +13,25 @@ class GaussianNoise(Transform):
         self.__mu = mu
         self.__sigma = sigma
 
-    def __call__(self, t: float, x: ArrayLike) -> ArrayLike:
-        assert np.shape(self.__mu) == np.shape(x), "Mu and x have to match in shape."
-
+    def __call__(self, _: float, x: ArrayLike) -> ArrayLike:
         if np.isscalar(x):
             noise = np.random.normal(0, 1)
             x += self.__sigma * noise + self.__mu
         else:
+            dim = len(x)
+
+            if np.isscalar(self.__sigma):
+                sigma = np.identity(dim) * self.__sigma
+            else:
+                sigma = self.__sigma
+                
+            if np.isscalar(self.__mu):
+                mu = np.ones(dim)
+            else:
+                mu = self.__mu
+
             noise = np.random.normal(0, 1, np.shape(x))
-            x += self.__sigma @ noise + self.__mu
+            x += sigma @ noise + mu
 
         return x