ENH: Fix Orientation Param of Inertial Sensors (#688)

* ENH: use 313 rotation

* TST: fix tests

* DOC: fix notebook

* DEV: changelog

* MNT: black on notebooks

Author: MateusStano

CHANGELOG.md

@@ -32,7 +32,8 @@ Attention: The newest changes should be on top -->
 
 ### Added
 
-- ENH: Adds Sensors classes [683](https://github.com/RocketPy-Team/RocketPy/pull/683)
+- ENH: Fix Orientation Param of Inertial Sensors [#688](https://github.com/RocketPy-Team/RocketPy/pull/688)
+- ENH: Adds Sensors classes [#683](https://github.com/RocketPy-Team/RocketPy/pull/683)
 - DOC: Cavour Flight Example [#682](https://github.com/RocketPy-Team/RocketPy/pull/682)
 - DOC: Halcyon Flight Example [#681](https://github.com/RocketPy-Team/RocketPy/pull/681)
 - ENH: Adds GenericMotor.load_from_eng_file() method [#676](https://github.com/RocketPy-Team/RocketPy/pull/676)

docs/notebooks/sensors.ipynb

@@ -1,3 +1,5 @@
+.. _rocket_axes:
+
 Rocket Class Axes Definitions
 =============================
 

docs/user/rocket/rocket_axes.rst

@@ -2,7 +2,7 @@
 from functools import cached_property
 from itertools import product
 
-from rocketpy.tools import euler321_to_quaternions, normalize_quaternions
+from rocketpy.tools import euler313_to_quaternions, normalize_quaternions
 
 
 class Vector:
@@ -1063,25 +1063,26 @@ def transformation(quaternion):
         )
 
     @staticmethod
-    def transformation_euler_angles(roll, pitch, yaw):
+    def transformation_euler_angles(roll, pitch, roll2):
         """Returns the transformation Matrix from frame B to frame A, where B
-        is rotated by the Euler angles roll, pitch and yaw with respect to A.
+        is rotated by the Euler angles roll, pitch and roll2 in an instrinsic
+        3-1-3 sequence with respect to A.
 
         Parameters
         ----------
         roll : float
-            The roll angle in degrees.
+            The roll angle in radians.
         pitch : float
-            The pitch angle in degrees.
-        yaw : float
-            The yaw angle in degrees.
+            The pitch angle in radians.
+        roll2 : float
+            The roll2 angle in radians.
 
         Returns
         -------
         Matrix
             The transformation matrix from frame B to frame A.
         """
-        return Matrix.transformation(euler321_to_quaternions(roll, pitch, yaw))
+        return Matrix.transformation(euler313_to_quaternions(roll, pitch, roll2))
 
 
 if __name__ == "__main__":

rocketpy/mathutils/vector_matrix.py

@@ -308,7 +308,7 @@ class InertialSensor(Sensor):
     name : str
         The name of the sensor.
     rotation_matrix : Matrix
-        The rotation matrix of the sensor from the sensor frame to the rocket
+        The rotation matrix of the sensor from the rocket frame to the sensor
         frame of reference.
     normal_vector : Vector
         The normal vector of the sensor in the rocket frame of reference.
@@ -345,22 +345,21 @@ def __init__(
         sampling_rate : float
             Sample rate of the sensor
         orientation : tuple, list, optional
-            Orientation of the sensor in the rocket. The orientation can be
-            given as:
-            - A list of length 3, where the elements are the Euler angles for
-              the rotation yaw (ψ), pitch (θ) and roll (φ) in radians. The
-              standard rotation sequence is z-y-x (3-2-1) is used, meaning the
-              sensor is first rotated by ψ around the x axis, then by θ around
-              the new y axis and finally by φ around the new z axis.
+            Orientation of the sensor in relation to the rocket frame of
+            reference (Body Axes Coordinate System). See :ref:'rocket_axes' for
+            more information.
+            If orientation is not given, the sensor axes will be aligned with
+            the rocket axis.
+            The orientation can be given as:
+            - A list or tuple of length 3, where the elements are the intrisic
+              rotation angles in radians. The rotation sequence z-x-z (3-1-3) is
+              used, meaning the sensor is first around the z axis (roll), then
+              around the new x axis (pitch) and finally around the new z axis
+              (roll).
             - A list of lists (matrix) of shape 3x3, representing the rotation
               matrix from the sensor frame to the rocket frame. The sensor frame
-              of reference is defined as to have z axis along the sensor's normal
-              vector pointing upwards, x and y axes perpendicular to the z axis
-              and each other.
-            The rocket frame of reference is defined as to have z axis
-            along the rocket's axis of symmetry pointing upwards, x and y axes
-            perpendicular to the z axis and each other. Default is (0, 0, 0),
-            meaning the sensor is aligned with the rocket's axis of symmetry.
+              of reference is defined as being initially aligned with the rocket
+              frame of reference.
         measurement_range : float, tuple, optional
             The measurement range of the sensor in the sensor units. If a float,
             the same range is applied both for positive and negative values. If
@@ -463,7 +462,7 @@ def __init__(
             self.rotation_matrix = Matrix(orientation)
         elif len(orientation) == 3:  # euler angles
             self.rotation_matrix = Matrix.transformation_euler_angles(
-                *orientation
+                *np.deg2rad(orientation)
             ).round(12)
         else:
             raise ValueError("Invalid orientation format")

rocketpy/sensors/sensor.py

@@ -1107,48 +1107,14 @@ def normalize_quaternions(quaternions):
     return q_w / q_norm, q_x / q_norm, q_y / q_norm, q_z / q_norm
 
 
-def euler321_to_quaternions(psi, theta, phi):
-    """Calculates the quaternions (Euler parameters) from the Euler angles in
-    yaw, pitch, and roll sequence (3-2-1).
-
-    Parameters
-    ----------
-    psi : float
-        Euler angle due to roll in degrees, also known as the spin angle.
-    theta : float
-        Euler angle due to pitch in degrees, also known as the nutation angle.
-    phi : float
-        Euler angle due to yaw in degrees, also known as the precession angle.
-
-    Returns
-    -------
-    tuple[float, float, float, float]
-        Tuple containing the Euler parameters e0, e1, e2, e3
-    """
-    phi = math.radians(phi)
-    theta = math.radians(theta)
-    psi = math.radians(psi)
-    cr = math.cos(phi / 2)
-    sr = math.sin(phi / 2)
-    cp = math.cos(theta / 2)
-    sp = math.sin(theta / 2)
-    cy = math.cos(psi / 2)
-    sy = math.sin(psi / 2)
-    e0 = cr * cp * cy + sr * sp * sy
-    e1 = sr * cp * cy - cr * sp * sy
-    e2 = cr * sp * cy + sr * cp * sy
-    e3 = cr * cp * sy - sr * sp * cy
-    return e0, e1, e2, e3
-
-
 def euler313_to_quaternions(phi, theta, psi):
     """Convert 3-1-3 Euler angles to Euler parameters (quaternions).
 
     Parameters
     ----------
     phi : float
         Rotation angle around the z-axis (in radians). Represents the precession
-        angle or the yaw angle.
+        angle or the roll angle.
     theta : float
         Rotation angle around the x-axis (in radians). Represents the nutation
         angle or the pitch angle.
@@ -1165,18 +1131,16 @@ def euler313_to_quaternions(phi, theta, psi):
     ----------
     https://www.astro.rug.nl/software/kapteyn-beta/_downloads/attitude.pdf
     """
-    e0 = np.cos(phi / 2) * np.cos(theta / 2) * np.cos(psi / 2) - np.sin(
-        phi / 2
-    ) * np.cos(theta / 2) * np.sin(psi / 2)
-    e1 = np.cos(phi / 2) * np.cos(psi / 2) * np.sin(theta / 2) + np.sin(
-        phi / 2
-    ) * np.sin(theta / 2) * np.sin(psi / 2)
-    e2 = np.cos(phi / 2) * np.sin(theta / 2) * np.sin(psi / 2) - np.sin(
-        phi / 2
-    ) * np.cos(psi / 2) * np.sin(theta / 2)
-    e3 = np.cos(phi / 2) * np.cos(theta / 2) * np.sin(psi / 2) + np.cos(
-        theta / 2
-    ) * np.cos(psi / 2) * np.sin(phi / 2)
+    cphi = np.cos(phi / 2)
+    sphi = np.sin(phi / 2)
+    ctheta = np.cos(theta / 2)
+    stheta = np.sin(theta / 2)
+    cpsi = np.cos(psi / 2)
+    spsi = np.sin(psi / 2)
+    e0 = cphi * ctheta * cpsi - sphi * ctheta * spsi
+    e1 = cphi * cpsi * stheta + sphi * stheta * spsi
+    e2 = cphi * stheta * spsi - sphi * cpsi * stheta
+    e3 = cphi * ctheta * spsi + ctheta * cpsi * sphi
     return e0, e1, e2, e3
 
 

rocketpy/tools.py

@@ -6,7 +6,7 @@
 from pytest import approx
 
 from rocketpy.mathutils.vector_matrix import Matrix, Vector
-from rocketpy.tools import euler321_to_quaternions
+from rocketpy.tools import euler313_to_quaternions
 
 # calisto standard simulation no wind solution index 200
 TIME = 3.338513236767685
@@ -71,9 +71,9 @@ def test_rotation_matrix(noisy_rotated_accelerometer):
     # values from external source
     expected_matrix = np.array(
         [
-            [0.2500000, -0.0580127, 0.9665064],
-            [0.4330127, 0.8995190, -0.0580127],
-            [-0.8660254, 0.4330127, 0.2500000],
+            [-0.125, -0.6495190528383292, 0.7499999999999999],
+            [0.6495190528383292, -0.625, -0.43301270189221946],
+            [0.7499999999999999, 0.43301270189221946, 0.5000000000000001],
         ]
     )
     rotation_matrix = np.array(noisy_rotated_accelerometer.rotation_matrix.components)
@@ -287,7 +287,9 @@ def test_noisy_rotated_accelerometer(noisy_rotated_accelerometer, example_plain_
             [0.005, 0.005, 1],
         ]
     )
-    sensor_rotation = Matrix.transformation(euler321_to_quaternions(60, 60, 60))
+    sensor_rotation = Matrix.transformation(
+        euler313_to_quaternions(*np.deg2rad([60, 60, 60]))
+    )
     total_rotation = sensor_rotation @ cross_axis_sensitivity
     rocket_rotation = Matrix.transformation(U[6:10])
     # expected measurement without noise
@@ -328,7 +330,9 @@ def test_noisy_rotated_gyroscope(noisy_rotated_gyroscope, example_plain_env):
             [0.005, 0.005, 1],
         ]
     )
-    sensor_rotation = Matrix.transformation(euler321_to_quaternions(-60, -60, -60))
+    sensor_rotation = Matrix.transformation(
+        euler313_to_quaternions(*np.deg2rad([-60, -60, -60]))
+    )
     total_rotation = sensor_rotation @ cross_axis_sensitivity
     rocket_rotation = Matrix.transformation(U[6:10])
     # expected measurement without noise

tests/unit/test_sensor.py

@@ -3,17 +3,17 @@
 
 from rocketpy.tools import (
     calculate_cubic_hermite_coefficients,
-    euler321_to_quaternions,
+    euler313_to_quaternions,
     find_roots_cubic_function,
 )
 
 
 @pytest.mark.parametrize(
     "angles, expected_quaternions",
-    [((0, 0, 0), (1, 0, 0, 0)), ((90, 90, 90), (0.7071068, 0, 0.7071068, 0))],
+    [((0, 0, 0), (1, 0, 0, 0)), ((90, 90, 90), (0, 0.7071068, 0, 0.7071068))],
 )
 def test_euler_to_quaternions(angles, expected_quaternions):
-    q0, q1, q2, q3 = euler321_to_quaternions(*angles)
+    q0, q1, q2, q3 = euler313_to_quaternions(*np.deg2rad(angles))
     assert round(q0, 7) == expected_quaternions[0]
     assert round(q1, 7) == expected_quaternions[1]
     assert round(q2, 7) == expected_quaternions[2]

tests/unit/test_tools.py

@@ -245,15 +245,15 @@ def test_matrix_transformation():
 
 
 def test_matrix_transformation_euler_angles():
-    phi = 0
-    theta = 0
-    psi = 90
-    matrix = Matrix.transformation_euler_angles(phi, theta, psi)
+    roll = np.pi / 2
+    pitch = np.pi / 2
+    roll2 = np.pi / 2
+    matrix = Matrix.transformation_euler_angles(roll, pitch, roll2)
     matrix = matrix.round(12)
     # Check that the matrix is orthogonal
     assert matrix @ matrix.transpose == Matrix.identity()
     # Check that the matrix rotates the vector correctly
-    assert matrix @ Vector([0, 0, 1]) == Vector([0, -1, 0])
+    assert matrix @ Vector([0, 0, 1]) == Vector([1, 0, 0])
 
 
 def test_matrix_round():