mirror of
https://git.intern.spaceteamaachen.de/ALPAKA/SPATZ.git
synced 2025-06-10 01:55:59 +00:00
86 lines
2.8 KiB
Python
86 lines
2.8 KiB
Python
import numpy as np
|
|
|
|
from typing import AnyStr, List
|
|
from numpy.typing import ArrayLike
|
|
|
|
from spatz.sensors import Accelerometer, Gyroscope, IMU, CoordSystem
|
|
from spatz.transforms import Transform, GaussianNoise, DriftingBias
|
|
from spatz.simulations.data_source import DataSource
|
|
from spatz.dataset import Dataset
|
|
from spatz.logger import Logger
|
|
|
|
|
|
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, 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.__constant_bias + self.__normal(t, self.__bias(t, x))
|
|
noise = y - x
|
|
|
|
# Log the chosen noise values.
|
|
self._logger.write('acc_x_noise', noise[0], self._get_name())
|
|
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, 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'
|
|
|
|
def _sensor_specific_effects(self, x: ArrayLike) -> ArrayLike:
|
|
# 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])
|
|
self._log('oy', x[1])
|
|
self._log('oz', x[2])
|
|
|
|
return x
|