Latest updates to data conversion

spatz/sensors/imu/accelerometer.py

@@ -32,7 +32,7 @@ class Accelerometer(Sensor):
             orientation: NDArray = np.identity(3), 
             offset: float = 0, 
             transforms: List[Transform] = []):
-        """_summary_
+        """Accelerometer sensor base class.
 
         Args:
             dataset (Dataset): A dataset object to fetch data from.
@@ -50,30 +50,8 @@ class Accelerometer(Sensor):
         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())
-
-        # Convert FL to body
-        acc = self._dataset.global_to_local() @ acc
-        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())
-
-        # Rotate the acceleration vector to accomodate the accelerometer's orientation on the rocket.
-        acc = self._orientation @ acc
-
-        # Add the effects of the imu's offset.
-        omega = self._dataset.get_angular_velocity()
-        acc += (np.cross(omega, self._offset) + np.cross(omega, np.cross(omega, self._offset)))
-        """
-
         acc = self._dataset.get_acceleration('local')
+        acc += self._dataset.global_to_local() @ np.array([g, 0, 0])
 
         return acc
     

spatz/sensors/imu/iam_20380ht.py

@@ -6,16 +6,26 @@ 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
+from spatz.transforms import Transform, GaussianNoise, DriftingBias
 
 
 class IAM_20380HT(Gyroscope):
     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, 0.00105, 0.00203]), 400)
+        self.__constant_bias = np.random.normal(0, 2, 3)
+        self.__normal = GaussianNoise(0, np.array([0.0049272, 0.00557833, 0.00407826]))
+
+
     def _get_name(self) -> AnyStr:
         return 'IAM-20380HT'
 
     def _sensor_specific_effects(self, x: ArrayLike) -> ArrayLike:
+        x = (x / np.pi) * 180
+
+        t = self._dataset.get_time()
+        x = self.__constant_bias + self.__normal(t, self.__bias(t, x))
+
         return x
     

spatz/sensors/imu/sca3300.py

@@ -13,7 +13,7 @@ class SCA3300(Accelerometer):
     def __init__(self, dataset: Dataset, logger: Logger, orientation=np.identity(3), offset=0, transforms: List[Transform] = []):
         super().__init__(dataset, logger, orientation, offset, transforms)
 
-        self.__noise = GaussianNoise(0, 0.001)
+        self.__noise = GaussianNoise(0, 0.01)
 
     def _get_name(self) -> AnyStr:
         return 'SCA3300'

spatz/sensors/pressure/pressure.py

@@ -34,7 +34,7 @@ class PressureSensor(Sensor):
         if self._ts_effects:
             # Pre-defined constants.
             _p = 3e6
-            sigma = 40
+            sigma = 100
 
             # How far away from transsonic speed (mach 1) are we?
             vvec = self._dataset.get_velocity('global')

spatz/simulations/astos_source.py

@@ -39,10 +39,10 @@ class ASTOSSource(CSVSource):
         super().__init__(path, 'time', interpolation)
 
     def get_position(self) -> NDArray:
-        return self.fetch_values(['x', 'y', 'z'])
+        return self.fetch_values(['px_L', 'py_L', 'pz_L'])
 
     def get_velocity(self, frame: Literal['global', 'local']) -> NDArray:
-        vel = self.fetch_values(['vx', 'vy', 'vz'])
+        vel = self.fetch_values(['vx_L', 'vy_L', 'vz_L'])
 
         if frame == 'local':
             return self.global_to_local() @ vel
@@ -50,10 +50,10 @@ class ASTOSSource(CSVSource):
         return vel
 
     def get_acceleration(self, frame: Literal['global', 'local']) -> NDArray:
-        acc = self.fetch_values(['ax', 'ay', 'az'])
-
-        if frame == 'local':
-            return self.global_to_local() @ acc
+        if frame == 'global':
+            acc = self.fetch_values(['ax_L', 'ay_L', 'az_L']) + np.array([-self.fetch_value('gravity'), 0, 0])
+        else:
+            acc = self.fetch_values(['ax_B', 'ay_B', 'az_B']) + self.fetch_init_values(['gx_B', 'gy_B', 'gz_B'])
         
         return acc
 
@@ -69,22 +69,7 @@ class ASTOSSource(CSVSource):
         pitch_l, yaw_l, roll_l = rots[0], rots[1], rots[2]
         local_to_body = T1(roll_l) @ T2(pitch_l - math.pi/2) @ T1(-yaw_l)
 
-        # ASTOS global to launch rail.
-        init_long = self.fetch_init_value('longitude')
-        init_lat = self.fetch_init_value('latitude')
-        global_to_launch_rail = T2(-math.pi/2 - init_lat) @ T3(init_long)
-
-        # ASTOS global to local.
-        decl = self.fetch_value('declination')
-        long = self.fetch_value('longitude')
-        t0 = self.get_start_time()
-        omega_E = (2*math.pi) / (24*60*60)
-        global_to_local = T2(-decl) @ T3(long + omega_E * t0)
-
-        # ASTOS local to launch rail inverted
-        local_to_launch_rail_inv = global_to_launch_rail @ global_to_local
-
-        return local_to_body @ local_to_launch_rail_inv
+        return local_to_body
     
     def get_angular_velocity(self) -> NDArray:
         return self.fetch_values(['OMEGA_X', 'OMEGA_Y', 'OMEGA_Z'])

testing.py

@@ -1,148 +0,0 @@
-import pygame
-import serial
-import re
-import numpy as np
-import time
-
-from numpy import array
-from pyquaternion import Quaternion
-from math import cos, sin, pi
-
-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()