diff --git a/rocketpy/mathutils/vector_matrix.py b/rocketpy/mathutils/vector_matrix.py index 8263da2bf..b8640ddea 100644 --- a/rocketpy/mathutils/vector_matrix.py +++ b/rocketpy/mathutils/vector_matrix.py @@ -242,6 +242,29 @@ def __xor__(self, other): ] ) + def __and__(self, other): + """Element wise multiplication between two R3 vectors. + + Parameters + ---------- + other : Vector + R3 vector to be multiplied with self. + + Returns + ------- + Vector + R3 vector resulting from the element wise multiplication between + self and other. + + Examples + -------- + >>> v = Vector([1, 7, 3]) + >>> u = Vector([2, 5, 6]) + >>> (v & u) + Vector(2, 35, 18) + """ + return Vector([self.x * other[0], self.y * other[1], self.z * other[2]]) + def __matmul__(self, other): """Dot product between two R3 vectors.""" return self.x * other.x + self.y * other.y + self.z * other.z diff --git a/rocketpy/sensors/accelerometer.py b/rocketpy/sensors/accelerometer.py index 1774fde76..1b8dd8680 100644 --- a/rocketpy/sensors/accelerometer.py +++ b/rocketpy/sensors/accelerometer.py @@ -59,27 +59,38 @@ def __init__( resolution : float, optional The resolution of the sensor in m/s^2/LSB. Default is 0, meaning no quantization is applied. - noise_density : float, optional + noise_density : float, list, optional The noise density of the sensor in m/s^2/√Hz. Sometimes called "white noise drift", "angular random walk" for gyroscopes, "velocity - random walk" for the accelerometers or "(rate) noise density". - Default is 0, meaning no noise is applied. - random_walk : float, optional + random walk" for the accelerometers or "(rate) noise density". If a + float or int is given, the same noise density is applied to all + axes. The values of each axis can be set individually by passing a + list of length 3. + random_walk : float, list, optional The random walk of the sensor in m/s^2/√Hz. Sometimes called "bias (in)stability" or "bias drift"". Default is 0, meaning no random - walk is applied. - constant_bias : float, optional + walk is applied. If a float or int is given, the same random walk is + applied to all axes. The values of each axis can be set individually + by passing a list of length 3. + constant_bias : float, list, optional The constant bias of the sensor in m/s^2. Default is 0, meaning no - constant bias is applied. + constant bias is applied. If a float or int is given, the same bias + is applied to all axes. The values of each axis can be set + individually by passing a list of length 3. operating_temperature : float, optional The operating temperature of the sensor in degrees Celsius. At 25°C, the temperature bias and scale factor are 0. Default is 25. - temperature_bias : float, optional + temperature_bias : float, list, optional The temperature bias of the sensor in m/s^2/°C. Default is 0, - meaning no temperature bias is applied. - temperature_scale_factor : float, optional + meaning no temperature bias is applied. If a float or int is given, + the same temperature bias is applied to all axes. The values of each + axis can be set individually by passing a list of length 3. + temperature_scale_factor : float, list, optional The temperature scale factor of the sensor in %/°C. Default is 0, - meaning no temperature scale factor is applied. + meaning no temperature scale factor is applied. If a float or int is + given, the same temperature scale factor is applied to all axes. The + values of each axis can be set individually by passing a list of + length 3. cross_axis_sensitivity : float, optional Skewness of the sensor's axes in percentage. Default is 0, meaning no cross-axis sensitivity is applied. @@ -93,9 +104,6 @@ def __init__( ------- None """ - self.type = "Accelerometer" - self.consider_gravity = consider_gravity - self.prints = _AccelerometerPrints(self) super().__init__( sampling_rate, orientation, @@ -110,6 +118,9 @@ def __init__( cross_axis_sensitivity=cross_axis_sensitivity, name=name, ) + self.type = "Accelerometer" + self.consider_gravity = consider_gravity + self.prints = _AccelerometerPrints(self) def measure(self, t, u, u_dot, relative_position, gravity, *args): """ diff --git a/rocketpy/sensors/gyroscope.py b/rocketpy/sensors/gyroscope.py index 21543e99c..be96c83d2 100644 --- a/rocketpy/sensors/gyroscope.py +++ b/rocketpy/sensors/gyroscope.py @@ -57,41 +57,53 @@ def __init__( resolution : float, optional The resolution of the sensor in rad/s/LSB. Default is 0, meaning no quantization is applied. - noise_density : float, optional + noise_density : float, list, optional The noise density of the sensor in rad/s/√Hz. Sometimes called "white noise drift", "angular random walk" for gyroscopes, "velocity random walk" for the accelerometers or "(rate) noise density". - Default is 0, meaning no noise is applied. - random_walk : float, optional + Default is 0, meaning no noise is applied. If a float or int is + given, the same noise density is applied to all axes. The values of + each axis can be set individually by passing a list of length 3. + random_walk : float, list, optional The random walk of the sensor in rad/s/√Hz. Sometimes called "bias (in)stability" or "bias drift"". Default is 0, meaning no random - walk is applied. - constant_bias : float, optional + walk is applied. If a float or int is given, the same random walk is + applied to all axes. The values of each axis can be set individually + by passing a list of length 3. + constant_bias : float, list, optional The constant bias of the sensor in rad/s. Default is 0, meaning no - constant bias is applied. + constant bias is applied. If a float or int is given, the same bias + is applied to all axes. The values of each axis can be set + individually by passing a list of length 3. operating_temperature : float, optional The operating temperature of the sensor in degrees Celsius. At 25°C, the temperature bias and scale factor are 0. Default is 25. - temperature_sensitivity : float, optional + temperature_sensitivity : float, list, optional The temperature bias of the sensor in rad/s/°C. Default is 0, - meaning no temperature bias is applied. - temperature_scale_factor : float, optional + meaning no temperature bias is applied. If a float or int is given, + the same temperature bias is applied to all axes. The values of each + axis can be set individually by passing a list of length 3. + temperature_scale_factor : float, list, optional The temperature scale factor of the sensor in %/°C. Default is 0, - meaning no temperature scale factor is applied. + meaning no temperature scale factor is applied. If a float or int is + given, the same temperature scale factor is applied to all axes. The + values of each axis can be set individually by passing a list of + length 3. cross_axis_sensitivity : float, optional Skewness of the sensor's axes in percentage. Default is 0, meaning no cross-axis sensitivity is applied. - acceleration_sensitivity : float, optional + of each axis can be set individually by passing a list of length 3. + acceleration_sensitivity : float, list, optional Sensitivity of the sensor to linear acceleration in rad/s/g. Default - is 0, meaning no sensitivity to linear acceleration is applied. + is 0, meaning no sensitivity to linear acceleration is applied. If a + float or int is given, the same sensitivity is applied to all axes. + The values of each axis can be set individually by passing a list of + length 3. Returns ------- None """ - self.type = "Gyroscope" - self.acceleration_sensitivity = acceleration_sensitivity - self.prints = _GyroscopePrints(self) super().__init__( sampling_rate, orientation, @@ -106,6 +118,11 @@ def __init__( cross_axis_sensitivity=cross_axis_sensitivity, name=name, ) + self.type = "Gyroscope" + self.acceleration_sensitivity = self._vectorize_input( + acceleration_sensitivity, "acceleration_sensitivity" + ) + self.prints = _GyroscopePrints(self) def measure(self, t, u, u_dot, relative_position, *args): """ @@ -126,7 +143,7 @@ def measure(self, t, u, u_dot, relative_position, *args): W = self.apply_temperature_drift(W) # Apply acceleration sensitivity - if self.acceleration_sensitivity != 0 and self.acceleration_sensitivity != None: + if self.acceleration_sensitivity != Vector.zeros(): W += self.apply_acceleration_sensitivity( omega, u_dot, relative_position, inertial_to_sensor ) @@ -174,7 +191,7 @@ def apply_acceleration_sensitivity( # Transform to sensor frame A = rotation_matrix @ A - return self.acceleration_sensitivity * A + return self.acceleration_sensitivity & A def export_measured_values(self, filename, format="csv"): """ diff --git a/rocketpy/sensors/sensors.py b/rocketpy/sensors/sensors.py index 8ba08317b..2813acbd8 100644 --- a/rocketpy/sensors/sensors.py +++ b/rocketpy/sensors/sensors.py @@ -55,27 +55,39 @@ def __init__( resolution : float, optional The resolution of the sensor in sensor units/LSB. Default is 0, meaning no quantization is applied. - noise_density : float, optional + noise_density : float, list, optional The noise density of the sensor in sensor units/√Hz. Sometimes called "white noise drift", "angular random walk" for gyroscopes, "velocity random walk" for the accelerometers or "(rate) noise density". Default is 0, meaning no noise is applied. - random_walk : float, optional + If a float or int is given, the same noise density is applied to all + axes. The values of each axis can be set individually by passing a + list of length 3. + random_walk : float, list, optional The random walk of the sensor in sensor units/√Hz. Sometimes called "bias (in)stability" or "bias drift"". Default is 0, meaning no - random walk is applied. - constant_bias : float, optional + random walk is applied. If a float or int is given, the same random + walk is applied to all axes. The values of each axis can be set + individually by passing a list of length 3. + constant_bias : float, list, optional The constant bias of the sensor in sensor units. Default is 0, - meaning no constant bias is applied. + meaning no constant bias is applied. If a float or int is given, the + same constant bias is applied to all axes. The values of each axis + can be set individually by passing a list of length 3. operating_temperature : float, optional The operating temperature of the sensor in degrees Celsius. At 25°C, the temperature bias and scale factor are 0. Default is 25. - temperature_bias : float, optional + temperature_bias : float, list, optional The temperature bias of the sensor in sensor units/°C. Default is 0, - meaning no temperature bias is applied. - temperature_scale_factor : float, optional + meaning no temperature bias is applied. If a float or int is given, + the same temperature bias is applied to all axes. The values of each + axis can be set individually by passing a list of length 3. + temperature_scale_factor : float, list, optional The temperature scale factor of the sensor in %/°C. Default is 0, - meaning no temperature scale factor is applied. + meaning no temperature scale factor is applied. If a float or int is + given, the same temperature scale factor is applied to all axes. The + values of each axis can be set individually by passing a list of + length 3. cross_axis_sensitivity : float, optional Skewness of the sensor's axes in percentage. Default is 0, meaning no cross-axis sensitivity is applied. @@ -89,12 +101,16 @@ def __init__( self.sampling_rate = sampling_rate self.orientation = orientation self.resolution = resolution - self.noise_density = noise_density * Vector([1, 1, 1]) - self.random_walk = random_walk * Vector([1, 1, 1]) - self.constant_bias = constant_bias * Vector([1, 1, 1]) self.operating_temperature = operating_temperature - self.temperature_bias = temperature_bias * Vector([1, 1, 1]) - self.temperature_scale_factor = temperature_scale_factor * Vector([1, 1, 1]) + self.noise_density = self._vectorize_input(noise_density, "noise_density") + self.random_walk = self._vectorize_input(random_walk, "random_walk") + self.constant_bias = self._vectorize_input(constant_bias, "constant_bias") + self.temperature_bias = self._vectorize_input( + temperature_bias, "temperature_bias" + ) + self.temperature_scale_factor = self._vectorize_input( + temperature_scale_factor, "temperature_scale_factor" + ) self.cross_axis_sensitivity = cross_axis_sensitivity self.name = name self._random_walk_drift = Vector([0, 0, 0]) @@ -138,11 +154,19 @@ def __init__( # compute total rotation matrix given cross axis sensitivity self._total_rotation_matrix = self.rotation_matrix @ _cross_axis_matrix + def _vectorize_input(self, value, name): + if isinstance(value, (int, float)): + return Vector([value, value, value]) + elif isinstance(value, (tuple, list)): + return Vector(value) + else: + raise ValueError(f"Invalid {name} format") + def __repr__(self): return f"{self.type} sensor, orientation: {self.orientation}" - def __call__(self, *args, **kwds): - return self.measure(*args, **kwds) + def __call__(self, *args, **kwargs): + return self.measure(*args, **kwargs) @abstractmethod def measure(self, *args, **kwargs): @@ -226,8 +250,6 @@ def apply_temperature_drift(self, value): Vector([1, 1, 1]) + (self.operating_temperature - 25) / 100 * self.temperature_scale_factor ) - value.x *= scale_factor.x - value.y *= scale_factor.y - value.z *= scale_factor.z + value = value & scale_factor return value diff --git a/tests/fixtures/sensors/sensors_fixtures.py b/tests/fixtures/sensors/sensors_fixtures.py index d334a0a46..bbb83b805 100644 --- a/tests/fixtures/sensors/sensors_fixtures.py +++ b/tests/fixtures/sensors/sensors_fixtures.py @@ -6,16 +6,16 @@ def noisy_rotated_accelerometer(): """Returns an accelerometer with all parameters set to non-default values, i.e. with noise and rotation.""" - # mpu6050 values + # mpu6050 approx values return Accelerometer( sampling_rate=100, orientation=(60, 60, 60), - noise_density=0.05, - random_walk=0.02, - constant_bias=0.5, + noise_density=[0, 0.03, 0.05], + random_walk=[0, 0.01, 0.02], + constant_bias=[0, 0.3, 0.5], operating_temperature=25, - temperature_bias=0.02, - temperature_scale_factor=0.02, + temperature_bias=[0, 0.01, 0.02], + temperature_scale_factor=[0, 0.01, 0.02], cross_axis_sensitivity=0.5, consider_gravity=True, name="Accelerometer", @@ -26,18 +26,18 @@ def noisy_rotated_accelerometer(): def noisy_rotated_gyroscope(): """Returns a gyroscope with all parameters set to non-default values, i.e. with noise and rotation.""" - # mpu6050 values + # mpu6050 approx values return Gyroscope( sampling_rate=100, orientation=(-60, -60, -60), - noise_density=0.05, - random_walk=0.02, - constant_bias=0.5, + noise_density=[0, 0.03, 0.05], + random_walk=[0, 0.01, 0.02], + constant_bias=[0, 0.3, 0.5], operating_temperature=25, - temperature_bias=0.02, - temperature_scale_factor=0.02, + temperature_bias=[0, 0.01, 0.02], + temperature_scale_factor=[0, 0.01, 0.02], cross_axis_sensitivity=0.5, - acceleration_sensitivity=0.0017, + acceleration_sensitivity=[0, 0.0008, 0.0017], name="Gyroscope", )