ALPAKA/SPATZ
1
0
Fork 0
mirror of https://git.intern.spaceteamaachen.de/ALPAKA/SPATZ.git synced 2025-12-18 01:48:03 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Updated sensors

Browse Source
This commit is contained in:
dario
2024-06-20 18:13:59 +02:00
parent c0ccd93acf
commit 43bc71c742
11 changed files with 57112 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
spatz/sensors/imu/wsen_isds.py
View File

@@ -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])