Merge pull request 'feature/ambiance' (#6) from feature/ambiance into main

Reviewed-on: https://git.intern.spaceteamaachen.de/ALPAKA/SPATZ/pulls/6

.vscode/launch.json

@@ -9,7 +9,8 @@
             "type": "debugpy",
             "request": "launch",
             "program": "testing.py",
-            "console": "integratedTerminal"
+            "console": "integratedTerminal",
+            "justMyCode": false
         }
     ]
 }

rework_test.py

@@ -0,0 +1,47 @@
+import cProfile
+import numpy as np
+import tqdm
+import math
+import matplotlib.pyplot as plt
+
+from spatz.simulation import Simulation
+from spatz.simulations.rocketpy import RocketPyCSV
+from spatz.sensors.pressure.ms5611 import MS5611
+from spatz.sensors.imu.wsen_isds import WSEN_ISDS_ACC, WSEN_ISDS_GYRO
+from spatz.sensors.imu.h3lis100dl import H3LIS100DL
+from spatz.sensors.gps.erinome1 import Erinome_I
+from spatz.observer import Observer
+
+
+simulation = Simulation().load(RocketPyCSV('nominal_wind.csv'))
+
+barometer = simulation.add_sensor(MS5611)
+
+# Orientation matrix of the WSEN-ISDS IMU on the rocket.
+orientation = np.array([
+    [ 0,  1,  0],
+    [ 0,  0,  1],
+    [-1,  0,  0]
+])
+accelerometer = simulation.add_sensor(WSEN_ISDS_ACC, orientation=orientation)
+gyroscope = simulation.add_sensor(WSEN_ISDS_GYRO)
+
+orientation = np.array([
+    [0, 1,  0],
+    [0, 0, -1],
+    [1, 0,  0]
+])
+high_g = simulation.add_sensor(H3LIS100DL)
+erinome = simulation.add_sensor(Erinome_I)
+
+
+for i in tqdm.tqdm(range(5000)):
+    simulation.advance(0.1)
+    pressure = barometer()
+    acc = accelerometer()
+    omegas = gyroscope()
+    g = high_g()
+    gps = erinome()
+
+df = simulation.get_logger().get_dataframe()
+df.to_csv('dummy2.csv')

siemens_parsed.csv

@@ -0,0 +1,91 @@
+time,DB5 Objekte_-ST3_Isttemperatur [°C],DB5 Objekte_-V2_Rückmeldung Arbeitsstellung für Trend,DB5 Objekte_-SV1_Istdruck Visu [mBar abs],DB5 Objekte_-SV1_Istdruck Visu Mantisse [mBar abs],DB5 Objekte_-SV1_Istdruck Visu Exponent,DB5 Objekte_-ST1_Isttemperatur [°C],DB5 Objekte_-ST2_Isttemperatur [°C],Pressure
+578.7037048339844,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
+1273.2175903320312,24.4,-20.0,902.03241,9.020324,2.0,24.4,25.0,902.0324
+1967.7777786254883,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
+2662.1527786254883,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
+3356.7013778686523,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
+4051.1342544555664,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
+4745.648147583008,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
+5440.127311706543,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
+6134.5601806640625,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
+6829.108787536621,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
+7523.611106872559,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
+8218.009262084961,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
+8912.488418579102,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
+9607.002311706543,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
+10301.493064880371,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
+10995.9375,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
+11690.46297454834,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
+12384.953704833984,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
+13079.456024169922,24.4,-20.0,902.03241,9.020324,2.0,24.4,25.0,902.0324
+13773.888885498047,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
+14468.298606872559,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
+15162.789352416992,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
+15857.291664123535,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
+16551.71295928955,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
+17246.30786895752,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
+17940.706024169922,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
+18635.150466918945,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
+19329.733795166016,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
+20024.201377868652,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
+20718.622688293457,24.299999,-20.0,923.56897,9.23569,2.0,24.200001,24.9,923.569
+21413.17130279541,24.299999,-20.0,233.712646,2.337126,2.0,23.6,24.200001,233.7126
+22107.673614501953,24.299999,-20.0,74.985168,7.498517,1.0,23.6,24.200001,74.98517
+22802.1759185791,24.299999,-20.0,29.057066,2.905707,1.0,23.700001,24.299999,29.05707
+23496.527770996094,24.299999,-20.0,12.757988,1.275799,1.0,23.799999,24.4,12.75799
+24190.94906616211,24.299999,-20.0,269.259064,2.692591,2.0,24.1,24.6,269.2591
+24885.48610687256,24.4,-20.0,923.56897,9.23569,2.0,24.299999,24.9,923.569
+25579.90739440918,24.4,-20.0,923.56897,9.23569,2.0,24.4,24.9,923.569
+26274.456024169922,24.4,-20.0,923.56897,9.23569,2.0,24.299999,24.9,923.569
+26968.90045928955,24.4,-20.0,923.56897,9.23569,2.0,24.299999,24.9,923.569
+27663.495376586914,24.4,-20.0,923.56897,9.23569,2.0,24.299999,24.9,923.569
+28357.98610687256,24.4,-20.0,923.56897,9.23569,2.0,24.299999,24.9,923.569
+29052.46527862549,24.4,-20.0,923.56897,9.23569,2.0,24.299999,24.9,923.569
+29746.94443511963,24.4,-20.0,923.56897,9.23569,2.0,24.299999,24.9,923.569
+30441.354164123535,24.4,-20.0,923.56897,9.23569,2.0,24.299999,24.9,923.569
+31135.76389312744,24.4,-20.0,923.56897,9.23569,2.0,24.299999,24.9,923.569
+31830.34722137451,24.4,-20.0,923.56897,9.23569,2.0,24.299999,24.9,923.569
+32524.872680664062,24.4,-20.0,923.56897,9.23569,2.0,24.299999,24.9,923.569
+33219.24768066406,24.4,-20.0,923.56897,9.23569,2.0,24.299999,24.9,923.569
+33913.784729003906,24.4,-20.0,923.56897,9.23569,2.0,24.299999,24.9,923.569
+34608.22917175293,24.4,-20.0,923.56897,9.23569,2.0,24.299999,24.9,923.569
+35302.800926208496,24.4,-20.0,923.56897,9.23569,2.0,24.299999,24.9,923.569
+35997.25694274902,24.299999,-20.0,548.038269,5.480383,2.0,23.799999,24.4,548.0382999999999
+36691.64351654053,24.299999,-20.0,145.58696,1.45587,2.0,23.5,24.1,145.587
+37386.18054962158,24.299999,-20.0,53.814953,5.381495,1.0,23.6,24.200001,53.81495
+38080.67130279541,24.299999,-20.0,21.891624,2.189162,1.0,23.700001,24.299999,21.89162
+38775.02314758301,24.299999,-20.0,9.773297,9.773297,0.0,23.799999,24.4,9.773297
+39469.60648345947,24.299999,-20.0,5.539739,5.539739,0.0,23.9,24.4,5.539739
+40164.10878753662,24.299999,-20.0,3.617636,3.617636,0.0,23.9,24.5,3.617636
+40858.541664123535,24.299999,-20.0,2.549855,2.549855,0.0,24.0,24.5,2.549855
+41553.02082824707,24.299999,-20.0,1.91839,1.91839,0.0,24.0,24.6,1.91839
+42247.534729003906,24.299999,-20.0,1.54061,1.54061,0.0,24.0,24.6,1.54061
+42942.00231170654,24.299999,-20.0,1.216785,1.216785,0.0,24.0,24.6,1.216785
+43636.48147583008,24.299999,-20.0,1.007466,1.007466,0.0,24.0,24.6,1.007466
+44330.92593383789,24.299999,-20.0,0.873253,8.732529,-1.0,24.0,24.6,0.8732529
+45025.42823791504,24.299999,-20.0,0.756922,7.569218,-1.0,24.0,24.6,0.7569218000000001
+45719.88426208496,24.299999,-20.0,0.656996,6.569963,-1.0,24.1,24.700001,0.6569963000000001
+46414.42130279541,24.299999,-20.0,0.597822,5.978216,-1.0,24.1,24.700001,0.5978216
+47108.93518066406,24.299999,-20.0,0.518181,5.181814,-1.0,24.1,24.700001,0.5181814
+47803.34490966797,24.299999,-20.0,0.47151,4.715096,-1.0,24.1,24.700001,0.47150960000000003
+48497.88194274902,24.299999,-20.0,0.429041,4.290406,-1.0,24.1,24.700001,0.4290406
+49192.36110687256,24.299999,-20.0,0.390398,3.903976,-1.0,24.1,24.700001,0.3903976
+49886.770835876465,24.299999,-20.0,0.354743,3.547427,-1.0,24.1,24.700001,0.3547427
+50581.25,24.299999,-20.0,0.322791,3.227909,-1.0,24.1,24.700001,0.3227909
+51275.76389312744,24.299999,-20.0,0.307912,3.079116,-1.0,24.1,24.700001,0.3079116
+51970.300926208496,24.299999,-20.0,0.280178,2.80178,-1.0,24.1,24.700001,0.280178
+52664.733795166016,24.299999,-20.0,0.254589,2.545894,-1.0,24.1,24.700001,0.2545894
+53359.20137786865,24.299999,-20.0,0.242853,2.428534,-1.0,24.1,24.700001,0.2428534
+54053.657402038574,24.299999,-20.0,0.22098,2.2098,-1.0,24.1,24.700001,0.22098
+54748.15971374512,24.299999,-20.0,0.210793,2.107933,-1.0,24.1,24.700001,0.21079330000000002
+55442.63888549805,24.299999,-20.0,0.193949,1.939492,-1.0,24.1,24.700001,0.19394920000000002
+56137.04860687256,24.299999,-20.0,0.182712,1.827117,-1.0,24.1,24.700001,0.18271170000000003
+56831.62036895752,24.299999,-20.0,0.174289,1.742891,-1.0,24.1,24.700001,0.1742891
+57526.08795928955,24.299999,-20.0,0.166255,1.662551,-1.0,24.1,24.700001,0.1662551
+58220.52082824707,24.299999,-20.0,0.164419,1.644192,-1.0,24.1,24.700001,0.16441920000000002
+58914.91898345947,24.299999,-20.0,0.169753,1.69753,-1.0,24.1,24.700001,0.16975300000000001
+59609.51387786865,24.299999,-20.0,0.174289,1.742891,-1.0,24.1,24.700001,0.1742891
+60303.912033081055,24.299999,-20.0,0.182712,1.827117,-1.0,24.1,24.700001,0.18271170000000003
+60998.530097961426,24.299999,-20.0,203.141159,2.031412,2.0,24.200001,24.799999,203.1412
+61692.90508270264,24.4,-20.0,632.268799,6.322688,2.0,24.299999,24.9,632.2688
+62387.44211578369,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569

spatz/__init__.py

@@ -8,4 +8,5 @@ import spatz.transforms as transforms
 from spatz.transforms import *
 
 from spatz.dataset import *
-from spatz.simulation import *
+from spatz.simulation import *
+from spatz.simulations import *

spatz/dataset.py

@@ -302,7 +302,7 @@ class Dataset(Advanceable):
         """
         return self.get_mach_number(t) > 1
 
-    def get_velocity(self, t: float | None = None) -> float:
+    def get_total_velocity(self, t: float | None = None) -> float:
         """
         Args:
             t (float): Allows specification of a different time instead of the current time. None for current time.

spatz/logger.py

@@ -1,51 +1,45 @@
 import numpy as np
 import pandas as pd
 
+from abc import abstractmethod
 from typing import Any, Tuple, List
 from numpy.typing import ArrayLike
 
-from abc import abstractmethod
+from spatz.simulations.advanceable import Advanceable
-
-
-class Advanceable:
-    def __init__(self) -> None:
-        self.reset()
-
-    def step(self, dt: float):
-        """Advances the simulation data in time.
-
-        Args:
-            dt (float): The step in time to make.
-        """
-        self.__t += dt
-        self._on_step(dt)
-
-    def reset(self):
-        """
-        Reset the Avanceable object to its initial state.
-        """
-        self.__t = 0
-        self._on_reset()
-
-    @abstractmethod
-    def _on_step(self, dt: float):
-        pass
-
-    @abstractmethod
-    def _on_reset(self):
-        pass
-
-    def get_time(self) -> float:
-        """
-        Returns:
-            float: Returns the current time of the Advanceable.
-        """
-        return self.__t
 
 
 class Logger(Advanceable):
     def __init__(self) -> None:
         super().__init__()
+
+    @abstractmethod
+    def write(self, attrib: str | List[str], value: Any | List[Any] | List[ArrayLike], domain: str = 'all'):
+        pass
+        
+
+class EmptyLogger(Logger):
+    def __init__(self) -> None:
+        super().__init__()
+
+        self.__idx = -1
+
+    def _on_step(self, _: float):
+        pass
+
+    def _on_reset(self):
+        pass
+
+    def write(self, attrib: str | List[str], value: Any | List[Any] | List[ArrayLike], domain: str = 'all'):
+        pass
+
+    def get_dataframe(self) -> pd.DataFrame:
+        pass
+
+
+class CSVLogger(Logger):
+    def __init__(self) -> None:
+        super().__init__()
+
         self.__idx = -1
 
     def _on_step(self, _: float):

spatz/models/pressure.py

@@ -2,6 +2,7 @@ from typing import Any
 import numpy as np
 
 from enum import Enum
+from ambiance import Atmosphere
 
 
 class PressUnit:
@@ -42,3 +43,11 @@ class AltitudeModel:
             press = to_hpa[unit]
 
         return 44330 * (1 - (press / self.__press_0)**(1 / 5.255))
+
+
+class StandardAtmosphere:
+    def __init__(self) -> None:
+        pass
+
+    def predict(self, press: float, unit: PressUnit = PressUnit.Pa) -> float:
+        return Atmosphere.from_pressure(press).h[0]

spatz/sensors/__init__.py

@@ -1,5 +1,6 @@
 from spatz.sensors.sensor import Sensor
 from spatz.sensors.gps import GPS
-from spatz.sensors.imu import Accelerometer, Gyroscope, IMU
+from spatz.sensors.imu import Accelerometer, Gyroscope, IMU, CoordSystem
 from spatz.sensors.pressure import PressureSensor
+from spatz.sensors.temperature import TemperatureSensor
 from spatz.sensors.compound import CompoundSensor

spatz/sensors/gps/erinome1.py

@@ -1,16 +1,20 @@
 from typing import List
 
 from numpy.typing import ArrayLike
-from sensors.gps import GPS
+from spatz.sensors.gps import GPS
 from spatz.dataset import ArrayLike, Dataset
+from spatz.simulations.data_source import DataSource
 from spatz.logger import ArrayLike, Logger
 from spatz.transforms import Transform
 
 
 class Erinome_I(GPS):
-    def __init__(self, dataset: Dataset, logger: Logger, transforms: List[Transform] = []):
+    def __init__(self, dataset: DataSource, logger: Logger, transforms: List[Transform] = []):
         super().__init__(dataset, logger, transforms)
 
+    def _get_name(self):
+        return "Erinome-I"
+
     def _sensor_specific_effects(self, x: ArrayLike) -> ArrayLike:
         # TODO: What's the GPS module's behavior?
 

spatz/sensors/gps/gps.py

@@ -1,3 +1,6 @@
+import math
+import numpy as np
+
 from typing import Any, List
 from numpy.linalg import norm
 
@@ -5,6 +8,7 @@ from numpy.typing import ArrayLike
 from pandas import NA
 
 from spatz.dataset import ArrayLike, Dataset
+from spatz.simulations.data_source import DataSource
 from spatz.logger import ArrayLike, Logger
 from spatz.sensors import Sensor
 from spatz.transforms import Transform
@@ -14,22 +18,24 @@ from spatz.transforms import Transform
 
 
 class GPS(Sensor):
-    def __init__(self, dataset: Dataset, logger: Logger, transforms: List[Transform] = []):
+    def __init__(self, dataset: DataSource, logger: Logger, transforms: List[Transform] = []):
         """GPS Module which provides the following information:
         - Longitude (in °)
         - Latitiude (in °)
         - Altitude (in m)
+        - Lock (1 or 0)
         """
         super().__init__(dataset, logger, transforms)
 
     def _get_data(self) -> ArrayLike:
-        vel = norm(self._dataset.get_velocity())
+        long = self._dataset.get_longitude()
+        lat = self._dataset.get_latitude()
+        alt = self._dataset.get_altitude()
+        lock = 1
 
-        # TODO: At which speed do we assume that GPS becomes unreliable?
+        self._log('longitude', long)
-        if vel / self._dataset.get_mach_number() > 1:
+        self._log('latitude', lat)
-            return NA
+        self._log('altitude', alt)
-        
+        self._log('lock', lock)
-        x = self._dataset.fetch_values(['latitude', 'longitude', 'altitude'])
-        x = self._sensor_specific_effects(x)
 
-        return x
+        return np.array([long, lat, alt, lock])

spatz/sensors/imu/__init__.py

@@ -1,3 +1,3 @@
-from spatz.sensors.imu.accelerometer import Accelerometer
+from spatz.sensors.imu.accelerometer import Accelerometer, CoordSystem
 from spatz.sensors.imu.gyroscope import Gyroscope
 from spatz.sensors.imu.imu import IMU

spatz/sensors/imu/accelerometer.py

@@ -1,11 +1,13 @@
 import numpy as np
 
 from typing import List
-from numpy.typing import ArrayLike
+from enum import Enum
+from numpy.typing import ArrayLike, NDArray
 
 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
 
 
@@ -18,32 +20,55 @@ __all__=[
 g = 9.81
 
 
+class CoordSystem:
+    LEFT_HANDED = 0,
+    RIGHT_HANDED = 0,
+
+
 class Accelerometer(Sensor):
-    def __init__(self, dataset: Dataset, logger: Logger, offset: float = 0, transforms: List[Transform] = []):
+    def __init__(
+            self, dataset: DataSource, 
+            logger: Logger, 
+            orientation: NDArray = np.identity(3), 
+            offset: float = 0, 
+            transforms: List[Transform] = []):
+        """_summary_
+
+        Args:
+            dataset (Dataset): A dataset object to fetch data from.
+            logger (Logger): A logger object to write data to.
+            coord_system (CoordSystem, optional): The type of coordinate system to use. Defaults to CoordSystem.RIGHT_HANDED.
+            orientation (NDArray, optional): The orientation of the sensor inside the spacecraft. Defaults to np.identity(3).
+            offset (float, optional): The offset of the sensor from the origin around which the object rotates. Defaults to 0.
+            transforms (List[Transform], optional): A list of transformations applied to the sensor measurements. Defaults to [].
+        """
         super().__init__(dataset, logger, transforms)
 
+        assert orientation.shape == (3, 3), 'Orientation has to be a 3x3 matrix.'
+
         self._offset = np.array([offset, 0, 0])
+        self._orientation = orientation
 
     def _get_data(self) -> ArrayLike | float:
-        acc = self._dataset.get_acceleration(frame='FL')
+        acc = self._dataset.get_acceleration('global')
-        acc += np.array([0, 0, g])
 
-        self._logger.write('FL_x', acc[0], self._get_name())
+        self._logger.write('global_ax', acc[0], self._get_name())
-        self._logger.write('FL_y', acc[1], self._get_name())
+        self._logger.write('global_ay', acc[1], self._get_name())
-        self._logger.write('FL_z', acc[2], self._get_name())
+        self._logger.write('global_az', acc[2], self._get_name())
 
         # Convert FL to body
-        acc = self._dataset.launch_rail_to_body() @ acc
+        acc = self._dataset.global_to_local() @ acc
+        acc += self._dataset.global_to_local() @ np.array([0, 0, g])
 
-        self._logger.write('B_x', acc[0], self._get_name())
+        self._logger.write('local_x', acc[0], self._get_name())
-        self._logger.write('B_y', acc[1], self._get_name())
+        self._logger.write('local_y', acc[1], self._get_name())
-        self._logger.write('B_z', acc[2], self._get_name())
+        self._logger.write('local_z', acc[2], self._get_name())
 
-        # Flip axes to sensor's perspective.
+        # Rotate the acceleration vector to accomodate the accelerometer's orientation on the rocket.
-        acc *= -1
+        acc = self._orientation @ acc
 
         # Add the effects of the imu's offset.
-        omega = self._dataset.get_angular_velocities()
+        omega = self._dataset.get_angular_velocity()
         acc += (np.cross(omega, self._offset) + np.cross(omega, np.cross(omega, self._offset)))
 
         return acc

spatz/sensors/imu/bmi088.py

@@ -0,0 +1,41 @@
+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, CoordSystem
+from spatz.transforms import Transform, GaussianNoise
+
+
+class BMI088(IMU):
+    def __init__(self, dataset: Dataset, logger: Logger, orientation = np.identity(3), offset=0, transforms: List[Transform] = []):
+        acc = BMI088Acc(dataset, logger, orientation, offset, transforms)
+        gyro = BMI088Gyro(dataset, logger, offset, transforms)
+
+        super().__init__(dataset, logger, acc, gyro, transforms)
+
+
+class BMI088Gyro(Gyroscope):
+    def __init__(self, dataset: Dataset, logger: Logger,  orientation=np.identity(3), transforms: List[Transform] = []):
+        super().__init__(dataset, logger, orientation, transforms)
+
+    def _get_name(self) -> AnyStr:
+        return 'BMI088'
+
+    def _sensor_specific_effects(self, x: ArrayLike) -> ArrayLike:
+        return x
+    
+
+class BMI088Acc(Accelerometer):
+    def __init__(self, dataset: Dataset, logger: Logger, orientation = np.identity(3), offset: float = 0, transforms: List[Transform] = []):
+        super().__init__(dataset, logger, CoordSystem.RIGHT_HANDED, orientation, offset, transforms)
+
+        self.__noise = GaussianNoise(0, 0.05)
+
+    def _get_name(self) -> AnyStr:
+        return 'BMI088'
+    
+    def _sensor_specific_effects(self, x: ArrayLike) -> ArrayLike:
+        return self.__noise(0, x)

spatz/sensors/imu/gyroscope.py

@@ -1,20 +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: Dataset, 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_rotation_rates()
+        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

@@ -0,0 +1,38 @@
+import numpy as np
+
+from typing import List, AnyStr
+from numpy.core.numeric import identity as identity
+from numpy.typing import ArrayLike, NDArray
+
+from spatz.dataset import Dataset
+from spatz.simulations.data_source import DataSource
+from spatz.logger import Logger
+from spatz.sensors import Accelerometer, CoordSystem
+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, orientation, offset, transforms)
+
+    def _get_name(self) -> AnyStr:
+        return "H3LIS100DL"
+    
+    def _sensor_specific_effects(self, x: ArrayLike) -> ArrayLike:
+        # The sensor only measures values between -100g and +100g and in g/LSB.
+        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/imu.py

@@ -12,7 +12,7 @@ class IMU(CompoundSensor):
                  dataset: Dataset, 
                  logger: Logger, 
                  acc: Accelerometer, 
-                 gyro: Gyroscope,
+                 gyro: Gyroscope,             
                  transforms: List[Transform] = []):
         """_summary_
 

spatz/sensors/imu/wsen_isds.py

@@ -3,8 +3,9 @@ import numpy as np
 from typing import AnyStr, List
 from numpy.typing import ArrayLike
 
-from spatz.sensors import Accelerometer, Gyroscope, IMU
+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
 
@@ -13,21 +14,37 @@ class WSEN_ISDS(IMU):
     pass
 
 
-class WSEN_ISDS_ACC(Accelerometer):
+g = 9.81
-    def __init__(self, dataset: Dataset, logger: Logger, offset: float, transforms: List[Transform] = []):
-        super().__init__(dataset, logger, offset, transforms)
 
-        self.__variance = 0.05
+
-        self.__noise = GaussianNoise(np.zeros(3), np.identity(3) * self.__variance)
+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, orientation, offset, transforms)
+
+        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()
+        # 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.
@@ -35,20 +52,34 @@ class WSEN_ISDS_ACC(Accelerometer):
         self._logger.write('acc_y_noise', noise[1], self._get_name())
         self._logger.write('acc_z_noise', noise[2], self._get_name())
 
+        # The WSEN-ISDS accelerometer only measures acceleration between -16g and 16g. 
+        y = np.clip(y, -16000, +16000)
+
         return y
     
 
 class WSEN_ISDS_GYRO(Gyroscope):
-    def __init__(self, dataset: Dataset, logger: Logger, offset: float, transforms: List[Transform] = []):
+    def __init__(self, dataset: Dataset, logger: Logger, orientation = np.identity(3), transforms: List[Transform] = []):
-        super().__init__(dataset, logger, offset, transforms)
+        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'
 
     def _sensor_specific_effects(self, x: ArrayLike) -> ArrayLike:
-        # Convert to degrees per second.
+        # Convert to milli-degrees per second.
-        x = (x / np.pi) * 180
+        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])
+        self._log('oy', x[1])
+        self._log('oz', x[2])
+
         return x

spatz/sensors/pressure/ms5611.py

@@ -0,0 +1,31 @@
+from typing import List, AnyStr
+from numpy.typing import ArrayLike
+
+from spatz.sensors import PressureSensor
+from spatz.dataset import Dataset, Phase
+from spatz.simulations.data_source import DataSource
+from spatz.logger import Logger
+from spatz.transforms import GaussianNoise, Transform, ProportionalGaussian
+
+
+class MS5611(PressureSensor):
+    def __init__(self, dataset: DataSource, logger: Logger, transforms: List[Transform] = [], ts_effects=True):
+        super().__init__(dataset, logger, transforms, ts_effects)
+
+        # 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 = GaussianNoise(0, 0.00043300242654881085)
+
+    def _get_name(self) -> AnyStr:
+        return 'MS5611'
+
+    def _sensor_specific_effects(self, x: ArrayLike | float) -> ArrayLike | float:
+        t = self._dataset.get_time()
+
+        noisy = self.__noise(t, x) # self.__pad_noise(t, x) if self._dataset.get_phase() == Phase.ONPAD else self.__flight_noise(t, x)
+
+        # Log the noise added to the pressure measurements.
+        self._logger.write('noise', noisy - x, domain=self._get_name())
+
+        return noisy

spatz/sensors/pressure/ms5611_01ba03.py

@@ -1,32 +0,0 @@
-from typing import List, AnyStr
-from numpy.typing import ArrayLike
-
-from spatz.sensors import PressureSensor
-from spatz.dataset import Dataset, Phase
-from spatz.logger import Logger
-from spatz.transforms import GaussianNoise, Transform
-
-
-class MS5611_01BA03(PressureSensor):
-    def __init__(self, dataset: Dataset, logger: Logger, transforms: List[Transform] = [], ts_effects=True):
-        super().__init__(dataset, logger, transforms, ts_effects)
-
-        # Noise model obtained by a test flight using this sensor.
-        self.__pad_noise = GaussianNoise(0, 0.03)
-        self.__flight_noise = GaussianNoise(0, 1.5)
-
-    def _get_name(self) -> AnyStr:
-        return 'MS5611_01BA03'
-
-    def _sensor_specific_effects(self, x: ArrayLike | float) -> ArrayLike | float:
-        t = self._dataset.get_time()
-
-        # Transform from Pa to hPa
-        x /= 1e2
-
-        noisy = self.__pad_noise(t, x) if self._dataset.get_phase() == Phase.ONPAD else self.__flight_noise(t, x)
-
-        # Log the noise added to the pressure measurements.
-        self._logger.write('noise', noisy - x, domain=self._get_name())
-
-        return noisy

spatz/sensors/pressure/pressure.py

@@ -6,11 +6,12 @@ from typing import List
 from spatz.sensors import Sensor
 from spatz.logger import Logger
 from spatz.dataset import Dataset
+from spatz.simulations.data_source import DataSource
 from spatz.transforms import Transform
 
 
 class PressureSensor(Sensor):
-    def __init__(self, dataset: Dataset, logger: Logger, transforms: List[Transform] = [], ts_effects=True, delay=0.0):
+    def __init__(self, dataset: DataSource, logger: Logger, transforms: List[Transform] = [], ts_effects=True, delay=0.0):
         """_summary_
 
         Args:
@@ -28,7 +29,7 @@ class PressureSensor(Sensor):
         self._ts_effects = active
 
     def _get_data(self) -> float:
-        x = self._dataset.get_pressure()
+        x = self._dataset.get_static_pressure()
 
         if self._ts_effects:
             # Pre-defined constants.
@@ -36,16 +37,16 @@ class PressureSensor(Sensor):
             sigma = 40
 
             # How far away from transsonic speed (mach 1) are we?
-            vvec = self._dataset.get_velocity()
+            vvec = self._dataset.get_velocity('global')
             dv = np.abs(np.linalg.norm(vvec) - self._dataset.get_speed_of_sound())
 
             # Model transsonic effects by a peak at mach 1 which decays the further we are away from it.
             ts_eff = _p * math.exp(-0.5* (dv / sigma)**2 ) / (sigma * math.sqrt(2*math.pi))
 
             # Log the values for the transsonic effect.
-            self._logger.write('ts_effects', ts_eff, domain=self._get_name())
+            self._log('ts_effects', ts_eff)
-            self._logger.write('mach_no', self._dataset.get_mach_number(), domain='mach')
+            self._log('mach_no', self._dataset.get_mach_number())
-            self._logger.write('speedofsound', self._dataset.get_speed_of_sound(), domain='mach')
+            self._log('speedofsound', self._dataset.get_speed_of_sound())
 
             x = x + ts_eff
 

spatz/sensors/sensor.py

@@ -4,21 +4,29 @@ import numpy as np
 from abc import abstractmethod
 from typing import List, AnyStr
 from numpy.typing import ArrayLike
+from pandas import NA
 
 from spatz.transforms import *
 from spatz.logger import *
 from spatz.dataset import *
+from spatz.simulations.data_source import DataSource
 
 
 class Sensor:
-    def __init__(self, dataset: Dataset, logger: Logger, transforms: List[Transform] = [], min_value=-np.inf, max_value=np.inf):
+    def __init__(
+            self, 
+            dataset: DataSource, 
+            logger: Logger, 
+            transforms: List[Transform] = [], 
+            min_value=-np.inf, 
+            max_value=np.inf):
         self._dataset = dataset
         self._logger = logger
         self._transforms = transforms
         self._min_value = min_value
         self._max_value = max_value
 
-    def set_dataset(self, dataset: Dataset):
+    def set_dataset(self, dataset: DataSource):
         self._dataset = dataset
 
     def set_logger(self, logger: Logger):
@@ -37,12 +45,12 @@ class Sensor:
     @abstractmethod
     def _sensor_specific_effects(self, x: ArrayLike | float) -> ArrayLike | float:
         raise NotImplementedError()
-
+    
     @abstractmethod
     def _get_data(self) -> ArrayLike | float:
         raise NotImplementedError()
-    
+
-    def get_init_data() -> ArrayLike:
+    def get_init_data(self) -> ArrayLike:
         pass
 
     def __call__(self) -> ArrayLike | float:

spatz/sensors/temperature/__init__.py

@@ -1 +1 @@
-from temperature import TemperatureSensor
+from spatz.sensors.temperature.temperature import TemperatureSensor

spatz/sensors/temperature/ms5611.py

@@ -0,0 +1,26 @@
+from numpy.typing import ArrayLike
+from typing import List, AnyStr
+
+from spatz.transforms.noise import GaussianNoise
+from spatz.dataset import Dataset
+from spatz.simulations.data_source import DataSource
+from spatz.sensors import TemperatureSensor
+from spatz.transforms import Transform
+
+
+class MS5611Temperature(TemperatureSensor):
+    def __init__(self, dataset: DataSource, transforms: List[Transform] = []):
+        super().__init__(dataset, transforms)
+
+        self.__noise = GaussianNoise(0, 0.5)
+
+    def _get_name(self) -> AnyStr:
+        return 'MS5611'
+
+    def _sensor_specific_effects(self, x: ArrayLike | float) -> ArrayLike | float:
+        t = self._dataset.get_time()
+        noisy = self.__noise(t, x)
+
+        self._log('temperature', noisy)
+
+        return noisy

spatz/simulation.py

@@ -2,11 +2,13 @@ from typing import List
 from numpy.random import normal
 from tqdm import tqdm
 
+from spatz.simulations.advanceable import Advanceable
+from spatz.simulations.data_source import DataSource
 from spatz.dataset import Dataset
 from spatz.logger import Logger
 from spatz.sensors import Sensor
 from spatz.dataset import Dataset, Phase
-from spatz.logger import Logger
+from spatz.logger import Logger, EmptyLogger
 from spatz.sensors import Sensor
 from spatz.observer import Observer
 
@@ -35,9 +37,11 @@ class UniformTimeSteps:
         return self.__dt + noise
 
 
-class Simulation:
+class Simulation(Advanceable):
     def __init__(self, time_steps=UniformTimeSteps(0.01)):
-        self.__dataset = None
+        super().__init__()
+
+        self.__data_source = None
         self.__logger = None
         self.__sensors: List[Sensor] = []
         self.__time_steps = time_steps
@@ -46,26 +50,25 @@ class Simulation:
         idx = 0
 
         # Clear all logs and reset the dataset to the first time step.
-        self.__dataset.reset()
+        self.__data_source.reset()
         self.__logger.reset()
 
         if verbose:
-            pbar = tqdm(total=self.__dataset.get_length())
+            pbar = tqdm(total=self.__data_source.get_length())
 
         while True:
-            t = self.__dataset.get_time()
+            t = self.get_time()
             dt = self.__time_steps(t)
             t_ = t + dt
             idx += 1
 
-            if t_ > self.__dataset.get_length():
+            if t_ > self.__data_source.get_length():
                 break
 
-            if until is not None and self.__dataset.get_phase() == until:
+            if until is not None and self.__data_source.get_phase() == until:
                 break
 
-            self.__dataset.step(dt)
+            self.advance(dt)
-            self.__logger.step(dt)
 
             if verbose:
                 pbar.update(dt)
@@ -75,22 +78,26 @@ class Simulation:
         if verbose:
             pbar.close()
 
-    def get_dataset(self) -> Dataset:
+    def _on_step(self, dt: float):
-        return self.__dataset
+        self.__data_source.advance(dt)
-    
+        self.__logger.advance(dt)
-    def get_logger(self) -> Logger:
-        return self.__logger
 
-    def load(self, path: str):
+    def load(self, source: DataSource):
-        self.__dataset = Dataset(path)
+        self.__data_source = source
-        self.__logger = Logger()
+        self.__logger = EmptyLogger()
         
         for sensor in self.__sensors:
-            sensor.set_dataset(self.__dataset)
+            sensor.set_dataset(self.__data_source)
             sensor.set_logger(self.__logger)
 
         return self
 
+    def get_data_source(self) -> DataSource:
+        return self.__data_source
+    
+    def get_logger(self) -> Logger:
+        return self.__logger
+
     def add_sensor(self, sensor, *args, **kwargs) -> Sensor:
         """Register a new sensor for this simulation. A registered sensor can be called like a function and returns
         the current measurements. The class' constructor arguments have to be given aswell.
@@ -103,7 +110,7 @@ class Simulation:
         """
         assert issubclass(sensor, Sensor), "Expected a subclass of Sensor."
 
-        self.__sensors.append(sensor(self.__dataset, self.__logger, *args, **kwargs))
+        self.__sensors.append(sensor(self.__data_source, self.__logger, *args, **kwargs))
 
         return self.__sensors[-1]
 
@@ -123,10 +130,10 @@ class Simulation:
             attributes = observer_or_attributes
             assert len(attributes) != 0, "Observed attributes list must be nonempty."
 
-            self.__sensors.append(Observer(self.__dataset, self.__logger, attributes))
+            self.__sensors.append(Observer(self.__data_source, self.__logger, attributes))
         else:
             observer = observer_or_attributes
-            self.__sensors.append(observer(self.__dataset, self.__logger))
+            self.__sensors.append(observer(self.__data_source, self.__logger))
 
         return self.__sensors[-1]
 

spatz/simulations/__init__.py

@@ -0,0 +1 @@
+from spatz.simulations.rocketpy import RocketPyCSV

spatz/simulations/advanceable.py

@@ -0,0 +1,47 @@
+from abc import abstractmethod
+
+
+class Advanceable:
+    def __init__(self) -> None:
+        self.reset()
+
+    def advance(self, dt: float):
+        """Advances the simulation data in time.
+
+        Args:
+            dt (float): The step in time to make.
+        """
+        self.__t += dt
+        self._on_step(dt)
+
+    def advance_to(self, t: float):
+        """Advances the simulation data to a new point in time.
+
+        Args:
+            t (float): The target point in time.
+        """
+        assert t > self.__t, 'Advanceable can only move forward in time.'
+
+        self.advance(t - self.__t)
+
+    def reset(self):
+        """
+        Reset the Avanceable object to its initial state.
+        """
+        self.__t = 0
+        self._on_reset()
+
+    @abstractmethod
+    def _on_step(self, dt: float):
+        pass
+
+    @abstractmethod
+    def _on_reset(self):
+        pass
+
+    def get_time(self) -> float:
+        """
+        Returns:
+            float: Returns the current time of the Advanceable.
+        """
+        return self.__t

spatz/simulations/astos_source.py

@@ -0,0 +1,53 @@
+import numpy as np
+
+from typing import Literal
+from numpy.typing import NDArray
+
+from spatz.simulations.csv_source import CSVSource
+
+
+class ASTOSSource(CSVSource):
+    def __init__(self, path: str, interpolation: Literal['linear'] = 'linear') -> None:
+        super().__init__(path, 'time', interpolation)
+
+    def get_length(self) -> float:
+        pass
+
+    def get_position(self) -> NDArray:
+        return self.fetch_values(['x', 'y', 'z'])
+
+    def get_velocity(self, frame: Literal['global', 'local']) -> NDArray:
+        if frame == 'local':
+            pass
+
+        return self.fetch_values(['vx', 'vy', 'vz'])
+
+    def get_acceleration(self, frame: Literal['global', 'local']) -> NDArray:
+        if frame == 'local':
+            pass
+
+        return self.fetch_values(['ax', 'ay', 'az'])
+
+    def get_attitude(self) -> NDArray:
+        pass
+
+    def local_to_global(self) -> NDArray:
+        pass
+    
+    def global_to_local(self) -> NDArray:
+        pass
+    
+    def get_angular_velocity(self) -> NDArray:
+        pass
+    
+    def get_static_pressure(self) -> float:
+        pass
+    
+    def get_longitude(self) -> float:
+        pass
+    
+    def get_latitude(self) -> float:
+        pass
+    
+    def get_altitude(self) -> float:
+        pass

spatz/simulations/csv_source.py

@@ -0,0 +1,91 @@
+import pandas as pd
+import numpy as np
+
+from typing import Literal, List
+from numpy.typing import NDArray
+
+from spatz.simulations.data_source import DataSource
+
+
+
+class CSVSource(DataSource):
+    def __init__(self, path: str, time_col: str, interpolation: Literal['linear']='linear') -> None:
+        """A data source that extracts all its data from a csv file.
+
+        Args:
+            time_col (str): The name of the column that contains time data.
+        """
+        super().__init__()
+
+        self._df = pd.read_csv(path)
+        self._time_col = time_col
+        self._idx = 0
+        self._interpolation = interpolation
+
+    def get_length(self) -> float:
+        return max(self._df[self._time_col])
+
+    def _on_reset(self):
+        pass
+
+    def _get_closest_idx(self, t: float) -> int:
+        """Gets an index _idx_ for the dataframe _df_ such that the values at the given time _t_ are somewhere between 
+        _idx_ and _idx+1_. 
+
+        Args:
+            t (float): The requested time.
+
+        Returns:
+            int: The computed index.
+        """
+        idx = (self._df[self._time_col] - t).abs().idxmin()
+        idx = idx if self._df[self._time_col].loc[idx] <= t else idx - 1
+
+        return idx
+
+    def _on_step(self, _: float):
+        self._idx = self._get_closest_idx(self.get_time())
+
+    def fetch_value(self, name: str, t: float | None = None, custom_interpolation=None) -> float:
+        """Get a specific value from the dataframe.
+
+        Args:
+            name (str): The name of the value to fetch.
+            t (float): Allows specification of a different time instead of the current time. None for current time.
+
+        Returns:
+            float: Returns the requested value.
+        """
+        idx = self._idx if t is None else self._get_closest_idx(t)
+
+        if self._interpolation == 'linear':
+            t_min = self._df.at[idx, self._time_col]
+            t_max = self._df.at[idx + 1, self._time_col]
+
+            # Sometimes no time passes in-between two samples.
+            if t_max == t_min:
+                return self._df.at[name, idx]
+            
+            # Compute the weight for interpolation.
+            alpha = (self.get_time() - t_min) / (t_max - t_min)
+
+            if custom_interpolation is not None:
+                a = self._df.at[idx, name]
+                b = self._df.at[idx + 1, name]
+
+                return custom_interpolation(a, b, alpha)
+
+            # Interpolate linearly between the two data points.
+            return (1 - alpha) * self._df.at[idx, name] + alpha * self._df.at[idx + 1, name]
+
+    def fetch_values(self, names: List[str], t: float | None = None, custom_interpolation=None) -> NDArray:
+        """Get specific values from the dataframe.
+
+        Args:
+            names (List[str]): Names of the values to get.
+            t (float): Allows specification of a different time instead of the current time. None for current time.
+
+        Returns:
+            np.array: Returns a numpy array containing the requested values in the same order as in the input list.
+        """
+        return np.asarray([self.fetch_value(name, t, custom_interpolation) for name in names])

spatz/simulations/data_source.py

@@ -0,0 +1,73 @@
+import numpy as np
+
+from spatz.simulations.advanceable import Advanceable
+
+from numpy.typing import NDArray
+from abc import abstractmethod
+from typing import Literal
+
+from ambiance import Atmosphere
+
+
+class DataSource(Advanceable):
+    def __init__(self) -> None:
+        super().__init__()
+
+    def get_speed_of_sound(self) -> float:
+        return Atmosphere(self.get_altitude()).speed_of_sound
+    
+    def get_mach_number(self) -> float:
+        speed = np.linalg.norm(self.get_velocity('global'))
+
+        return speed / self.get_speed_of_sound()
+    
+    def get_temperature(self) -> float:
+        return Atmosphere(self.get_altitude()).temperature
+
+    @abstractmethod
+    def get_length(self) -> float:
+        pass
+
+    @abstractmethod
+    def get_position(self) -> NDArray:
+        pass
+
+    @abstractmethod
+    def get_velocity(self, frame: Literal['global', 'local']) -> NDArray:
+        pass
+
+    @abstractmethod
+    def get_acceleration(self, frame: Literal['global', 'local']) -> NDArray:
+        pass
+
+    @abstractmethod
+    def get_attitude(self) -> NDArray:
+        pass
+
+    @abstractmethod
+    def local_to_global(self) -> NDArray:
+        pass
+
+    @abstractmethod
+    def global_to_local(self) -> NDArray:
+        pass
+
+    @abstractmethod
+    def get_angular_velocity(self) -> NDArray:
+        pass
+
+    @abstractmethod
+    def get_static_pressure(self) -> float:
+        pass
+
+    @abstractmethod
+    def get_longitude(self) -> float:
+        pass
+
+    @abstractmethod
+    def get_latitude(self) -> float:
+        pass
+
+    @abstractmethod
+    def get_altitude(self) -> float:
+        pass

spatz/simulations/rocketpy.py

@@ -0,0 +1,84 @@
+import numpy as np
+
+from typing import Literal
+from numpy.typing import NDArray
+
+from spatz.simulations.csv_source import CSVSource
+
+
+class RocketPyCSV(CSVSource):
+    def __init__(self, path: str, interpolation: Literal['linear'] = 'linear') -> None:
+        super().__init__(path, ' Time (s)', interpolation)
+
+    def get_position(self) -> NDArray:
+        return self.fetch_values([' X (m)', ' Y (m)', ' Z (m)'])
+
+    def get_velocity(self, frame: Literal['global', 'local']) -> NDArray:
+        vel_global = self.fetch_values([' Vx (m/s)', ' Vy (m/s)', ' Vz (m/s)'])
+
+        if frame == 'global':
+            return vel_global
+        
+        return self.global_to_local() @ vel_global
+
+    def get_acceleration(self, frame: Literal['global', 'local']) -> NDArray:
+        acc_global = self.fetch_values([' Ax (m/s²)', ' Ay (m/s²)', ' Az (m/s²)'])
+
+        if frame == 'global':
+            return acc_global
+        
+        return self.global_to_local() @ acc_global
+
+    def get_attitude(self) -> NDArray:
+        t_min = self._df.at[self._idx, self._time_col]
+        t_max = self._df.at[self._idx + 1, self._time_col]
+
+        def slerp(a, b, alpha):
+            theta = np.arccos(np.clip(np.dot(a, b), -1, 1))
+
+            if np.isclose(theta, 0):
+                return a
+                
+            return (a * np.sin((1-alpha) * theta) + b * np.sin(alpha * theta)) / np.sin(theta)
+        
+        qa = np.array([self._df.at[self._idx, ' e0'], self._df.at[self._idx, ' e1'], self._df.at[self._idx, ' e2'], self._df.at[self._idx, ' e3']])
+        qb = np.array([self._df.at[self._idx+1, ' e0'], self._df.at[self._idx+1, ' e1'], self._df.at[self._idx+1, ' e2'], self._df.at[self._idx+1, ' e3']])
+
+        alpha = (self.get_time() - t_min) / (t_max - t_min)
+
+        return slerp(qa, qb, alpha)
+
+    def global_to_local(self) -> NDArray:
+        quat = self.get_attitude()
+
+        e0, e1, e2, e3 = quat[0], quat[1], quat[2], quat[3]
+
+        # Taken from:
+        # https://docs.rocketpy.org/en/latest/technical/equations_of_motion.html
+        mat = np.array([
+            [e0**2 + e1**2 - e2**2 - e3**2, 2*(e1*e2+e0*e3), 2*(e1*e3 - e0*e2)], 
+            [2*(e1*e2 - e0*e3), e0**2 - e1**2 + e2**2 - e3**2, 2*(e2*e3 + e0*e1)], 
+            [2*(e1*e3 + e0*e2), 2*(e2*e3 - e0*e1), e0**2 - e1**2 - e2**2 + e3**2]
+        ])
+        
+        return mat
+
+    def local_to_global(self) -> NDArray:
+        return self.global_to_local().T
+
+    def get_angular_velocity(self) -> NDArray:
+        return self.fetch_values([' ω1 (rad/s)',' ω2 (rad/s)',' ω3 (rad/s)'])
+
+    def get_static_pressure(self) -> float:
+        return self.fetch_value(' Pressure (Pa)')
+
+    def get_longitude(self) -> float:
+        return self.fetch_value(' Longitude (°)')
+
+    def get_latitude(self) -> float:
+        return self.fetch_value(' Latitude (°)')
+
+    def get_altitude(self) -> float:
+        return self.fetch_value(' Z (m)')
+
+    

spatz/transforms/__init__.py

@@ -1,3 +1,3 @@
 from spatz.transforms.transform import Transform
-from spatz.transforms.noise import GaussianNoise
+from spatz.transforms.noise import GaussianNoise, ProportionalGaussian, DriftingBias
 from spatz.transforms.failures import Downtime

spatz/transforms/noise.py

@@ -7,7 +7,7 @@ from spatz.transforms import Transform
 
 
 class GaussianNoise(Transform):
-    def __init__(self, mu: ArrayLike, sigma: ArrayLike) -> None:
+    def __init__(self, mu: ArrayLike = None, sigma: ArrayLike = None) -> None:
         super().__init__()
 
         self.__mu = mu
@@ -15,7 +15,8 @@ class GaussianNoise(Transform):
 
     def __call__(self, _: float, x: ArrayLike) -> ArrayLike:
         if np.isscalar(x):
-            noise = np.random.normal(0, 1)
+            noise = np.random.normal(0, 1)         
+
             x += self.__sigma * noise + self.__mu
         else:
             dim = len(x)
@@ -26,7 +27,7 @@ class GaussianNoise(Transform):
                 sigma = self.__sigma
                 
             if np.isscalar(self.__mu):
-                mu = np.ones(dim)
+                mu = np.ones(dim) * self.__mu
             else:
                 mu = self.__mu
 
@@ -36,6 +37,50 @@ class GaussianNoise(Transform):
         return x
 
 
+class DriftingBias(Transform):
+    def __init__(self, init: ArrayLike, covariance: ArrayLike, Tc: float) -> None:
+        """First order Gauss-Markov (GM) model used to model drift.
+
+        Args:
+            init (ArrayLike): The initial bias.
+            covariance (ArrayLike): Covariance matrix of the process.
+            Tc (float): Correlation time of the process.
+        """
+        super().__init__()
+
+        self.__t = 0
+        self.__beta = 1 / Tc
+        self.__covariance = covariance
+        self.__Tc = Tc
+        self.__x_old = np.copy(init)
+
+    def __call__(self, t: float, x: ArrayLike) -> ArrayLike:
+        dt = t - self.__t
+        self.__t = t
+
+        w = np.random.normal(np.zeros_like(x), self.__covariance*(1-np.exp(-2 * dt / self.__Tc)))
+        drift = (1 - self.__beta * dt) * self.__x_old + w 
+        
+        self.__x_old = drift
+
+        return x + drift
+
+
+class ProportionalGaussian(Transform):
+    def __init__(self, mu, sigma) -> None:
+        super().__init__()
+
+        self.__mu = mu
+        self.__sigma = sigma
+
+    def __call__(self, _: float, x: ArrayLike) -> ArrayLike:
+        noise = np.random.normal(0, 1)         
+
+        x += (self.__sigma * x) * noise + (self.__mu * x)
+
+        return x
+
+
 class PinkNoise(Transform):
     def __init__(self) -> None:
         super().__init__()

testing.py

@@ -1,10 +1,148 @@
-from spatz.sensors.antenna.tx_gain import GainPattern
+import pygame
-import math
+import serial
+import re
+import numpy as np
+import time
 
-pattern = GainPattern("data/gain_pattern/farfield_all.txt")
+from numpy import array
+from pyquaternion import Quaternion
+from math import cos, sin, pi
 
-# pattern.get_gain(41,66)
+from spatz.sensors.imu.wsen_isds import WSEN_ISDS_GYRO
-# pattern.get_gain(40,100)
+from spatz.simulation import Simulation
-# pattern.get_gain(10,180)
+from spatz.simulations.rocketpy import RocketPyCSV
-# pattern.get_gain(0,95)
+
-# pattern.get_gain(21,100)
+# Blatantly stolen from: https://stackoverflow.com/questions/21019471/how-can-i-draw-a-3d-shape-using-pygame-no-other-modules
+
+X, Y, Z = 0, 1, 2
+
+
+def rotation_matrix(a, b, by):
+    """
+    rotation matrix of a, b, by radians around x, y, z axes (respectively)
+    """
+    sa, ca = sin(a), cos(a)
+    sb, cb = sin(b), cos(b)
+    sby, cby = sin(by), cos(by)
+    return (
+        (cb*cby, -cb*sby, sb),
+        (ca*sby + sa*sb*cby, ca*cby - sby*sa*sb, -cb*sa),
+        (sby*sa - ca*sb*cby, ca*sby*sb + sa*cby, ca*cb)
+    )
+
+
+class Physical:
+    def __init__(self, vertices, edges, colors):
+        """
+        a 3D object that can rotate around the three axes
+        :param vertices: a tuple of points (each has 3 coordinates)
+        :param edges: a tuple of pairs (each pair is a set containing 2 vertices' indexes)
+        """
+        self.__vertices = array(vertices)
+        self.__edges = tuple(edges)
+        self.__colors = tuple(colors)
+        self.__rotation = Quaternion(axis=(1, 0, 0), angle=0) # radians around each axis
+
+    def rotate(self, quaternion):
+        self.__rotation = quaternion
+
+    @property
+    def lines(self):
+        location = array([self.__rotation.rotate(vertex) for vertex in self.__vertices])  # an index->location mapping
+        return (((location[v1], location[v2]), color) for (v1, v2), color in zip(self.__edges, self.__colors))
+    
+
+BLACK, RED, GREEN, BLUE = (0, 0, 0), (255, 128, 128), (128, 255, 128), (128, 128, 255)
+
+LIGHTRED, LIGHTGREEN, LIGHTBLUE = (128, 64, 64), (64, 128, 64), (64, 64, 128)
+
+
+class Paint:
+    def __init__(self, shape, shape2):
+        self.__shape = shape
+        self.__shape2 = shape2
+        self.__size = 900, 450
+        self.__clock = pygame.time.Clock()
+        self.__screen = pygame.display.set_mode(self.__size)
+
+    def __fit(self, vec):
+        """
+        ignore the z-element (creating a very cheap projection), and scale x, y to the coordinates of the screen
+        """
+        # notice that len(self.__size) is 2, hence zip(vec, self.__size) ignores the vector's last coordinate
+        return [round(70 * coordinate + frame / 2) for coordinate, frame in zip(vec, self.__size)]
+
+    def __draw_shape(self, thickness=4):
+        for (start, end), color in self.__shape.lines:
+            pygame.draw.line(self.__screen, color, self.__fit((start[0]-2, start[1], start[2])), self.__fit((end[0]-2, end[1], end[2])), thickness)
+
+        for (start, end), color in self.__shape2.lines:
+            pygame.draw.line(self.__screen, color, self.__fit((start[0]+2, start[1], start[2])), self.__fit((end[0]+2, end[1], end[2])), thickness)
+
+    def draw(self):
+        self.__screen.fill(BLACK)
+        self.__draw_shape()
+        pygame.display.flip()
+        self.__clock.tick(40)          
+
+
+def main():
+    from pygame import K_q, K_w, K_a, K_s, K_z, K_x
+
+    rotation = Quaternion(axis=[1, 0, 0], angle=pi/2)
+
+    axes = Physical(
+        vertices=((0, 0, 0), (0, 0, 2), (0, 2, 0), (2, 0, 0)),
+        edges=({0, 1}, {0, 2}, {0, 3}),
+        colors=(BLUE, GREEN, RED)
+    ) 
+
+    truth = Physical(
+        vertices=((0, 0, 0), (0, 0, 2), (0, 2, 0), (2, 0, 0)),
+        edges=({0, 1}, {0, 2}, {0, 3}),
+        colors=(LIGHTBLUE, LIGHTGREEN, LIGHTRED)
+    ) 
+        
+    pygame.init()
+    pygame.display.set_caption("Simulation")
+    renderer = Paint(axes, truth)
+
+    simulation = Simulation().load(RocketPyCSV('nominal_wind.csv'))
+
+    gyro = simulation.add_sensor(WSEN_ISDS_GYRO)
+    offset = gyro.calibrate(100)
+    att = simulation.add_observer([' e0', ' e1', ' e2', ' e3'])
+    dt = 0.01
+
+    quat = np.array([-0.706864,0.018510,0.018510,-0.706864])
+
+    while True:
+        time.sleep(0.01)
+        simulation.advance(dt)
+        omegas = gyro() - offset
+        true = att()
+
+        omegas = (omegas / 1000) * np.pi / 180
+
+        matrix = np.array([
+            [1, -dt/2*omegas[0], -dt/2*omegas[1], -dt/2*omegas[2]],
+            [dt/2*omegas[0], 1, dt/2*omegas[2], -dt/2*omegas[1]],
+            [dt/2*omegas[1], -dt/2*omegas[2], 1, dt/2*omegas[0]],
+            [dt/2*omegas[2], dt/2*omegas[1], -dt/2*omegas[0], 1]
+        ])
+
+        quat = matrix @ quat
+        quat /= np.linalg.norm(quat)
+
+
+        for event in pygame.event.get():
+            if event.type == pygame.QUIT:
+                exit()
+            
+        axes.rotate(Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3]))
+        truth.rotate(Quaternion(x=true[0], y=true[1], z=true[2], w=true[3]))
+
+        renderer.draw()
+
+if __name__ == '__main__':
+    main()