Updated sensors

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/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'

spatz/transforms/__init__.py

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

spatz/transforms/noise.py

@@ -37,6 +37,35 @@ 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__()

testing.py

@@ -1,10 +1,148 @@
-from spatz.sensors.antenna.tx_gain import GainPattern
-import math
+import pygame
+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)
-# pattern.get_gain(40,100)
-# pattern.get_gain(10,180)
-# pattern.get_gain(0,95)
-# pattern.get_gain(21,100)
+from spatz.sensors.imu.wsen_isds import WSEN_ISDS_GYRO
+from spatz.simulation import Simulation
+from spatz.simulations.rocketpy import RocketPyCSV
+
+# 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()