ALPAKA/SPATZ
1
0
Fork 0
mirror of https://git.intern.spaceteamaachen.de/ALPAKA/SPATZ.git synced 2025-09-28 21:17:33 +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

20
spatz/sensors/imu/accelerometer.py
View File

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

19
spatz/sensors/imu/gyroscope.py
View File

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

10
spatz/sensors/imu/h3lis100dl.py
View File

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

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

2
spatz/sensors/pressure/ms5611.py
View File

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

2
spatz/transforms/__init__.py
View File

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

29
spatz/transforms/noise.py
View File

@@ -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__()