Coverage for src\attipy\_attitude.py: 83%
52 statements
« prev ^ index » next coverage.py v7.11.3, created at 2025-12-15 12:47 +0100
1from typing import Self
3import numpy as np
4from numpy.typing import ArrayLike, NDArray
6from ._transforms import (
7 _euler_zyx_from_quaternion,
8 _quaternion_from_euler_zyx,
9 _quaternion_from_matrix,
10 _rot_matrix_from_quaternion,
11)
14def _asarray_check_unit_quaternion(q: ArrayLike) -> NDArray[np.float64]:
15 """
16 Convert the input to a numpy array and check if it is a unit quaternion.
17 """
18 q = np.asarray_chkfinite(q, dtype=float)
19 if q.shape != (4,): 19 ↛ 20line 19 didn't jump to line 20 because the condition on line 19 was never true
20 raise ValueError("Unit quaternion must be a 4-element array.")
21 norm = np.linalg.norm(q)
22 if not np.isclose(norm, 1.0): 22 ↛ 23line 22 didn't jump to line 23 because the condition on line 22 was never true
23 raise ValueError("Unit quaternion must have a norm of 1.")
24 return q
27def _asarray_check_matrix_so3(A: ArrayLike) -> NDArray[np.float64]:
28 """
29 Convert the input to a numpy array and check if it is a valid rotation matrix
30 (element of SO(3)).
31 """
32 A = np.asarray_chkfinite(A, dtype=float)
33 if A.shape != (3, 3): 33 ↛ 34line 33 didn't jump to line 34 because the condition on line 33 was never true
34 raise ValueError("SO(3) matrix must be a 3x3 array.")
35 I3x3 = np.eye(3)
36 if not np.allclose(A.T @ A, I3x3): 36 ↛ 37line 36 didn't jump to line 37 because the condition on line 36 was never true
37 raise ValueError("SO(3) matrix must be orthogonal.")
38 if not np.isclose(np.linalg.det(A), 1.0): 38 ↛ 39line 38 didn't jump to line 39 because the condition on line 38 was never true
39 raise ValueError("SO(3) matrix must have a determinant of 1.")
40 return A
43class Attitude:
44 """
45 This class encapsulates the attitude (or rotation) of a 'body frame', {b}, relative
46 to a 'navigation frame', {n}. Although the {n} and {b} frames can be defined
47 arbitrarily, the main use case is for representing the attitude of a vehicle
48 or sensor (body frame) relative to a local-level inertial or global reference
49 frame (navigation frame) (e.g., North-East-Down, NED).
51 Internally, the attitude is represented using a unit quaternion, q, defined
52 such that it transforms a vector from the body frame to the navigation frame
53 using:
55 [0, v_n] = q ⊗ [0, v_b] ⊗ q*
57 where,
59 - q is the unit quaternion.
60 - q* is the conjugate of the unit quaternion q.
61 - v_b is a vector expressed in the body frame.
62 - v_n is the same vector expressed in the navigation frame.
64 and ⊗ denotes quaternion multiplication (Hamilton product).
66 Parameters
67 ----------
68 q : ArrayLike
69 The 4-element unit quaternion, [q_w, q_x, q_y, q_z], where q_w is the scalar
70 part, and q_x, q_y and q_z are the vector parts, respectively.
71 """
73 def __init__(self, q: ArrayLike) -> None:
74 self._q = _asarray_check_unit_quaternion(q)
76 def __repr__(self) -> str:
77 q_w, q_x, q_y, q_z = self._q
78 return f"Attitude(q=[{q_w:.3g} + {q_x:.3g}i + {q_y:.3g}j + {q_z:.3g}k])"
80 @classmethod
81 def from_quaternion(cls, q: ArrayLike) -> Self:
82 """
83 Create an Attitude instance from a unit quaternion, q, defined such that
84 it transforms a vector from the body frame, {b}, to the navigation frame,
85 {n}, using:
87 [0, v_n] = q ⊗ [0, v_b] ⊗ q*
89 where,
91 - q is the unit quaternion.
92 - q* is the conjugate of the unit quaternion q.
93 - v_b is a vector expressed in the body frame, {b}.
94 - v_n is the same vector expressed in the navigation frame, {n}.
96 and ⊗ denotes quaternion multiplication (Hamilton product).
98 Parameters
99 ----------
100 q : ArrayLike
101 The 4-element unit quaternion, [q_w, q_x, q_y, q_z], where q_w is the scalar
102 part, and q_x, q_y and q_z are the vector parts, respectively.
104 Returns
105 -------
106 Attitude
107 Attitude instance.
108 """
109 return cls(q)
111 def as_quaternion(self) -> NDArray[np.float64]:
112 """
113 Return the attitude as a unit quaternion, q, defined such that it transforms
114 a vector from the body frame, {b}, to the navigation frame, {n}, using:
116 [0, v_n] = q ⊗ [0, v_b] ⊗ q*
118 where,
120 - q is the unit quaternion.
121 - q* is the conjugate of the unit quaternion q.
122 - v_b is a vector expressed in the body frame, {b}.
123 - v_n is the same vector expressed in the navigation frame, {n}.
125 and ⊗ denotes quaternion multiplication (Hamilton product).
127 Returns
128 -------
129 numpy.ndarray, shape (4,)
130 The 4-element unit quaternion, [q_w, q_x, q_y, q_z], where q_w is the scalar
131 part, and q_x, q_y and q_z are the vector parts, respectively.
132 """
133 return self._q.copy()
135 @classmethod
136 def from_matrix(cls, A: ArrayLike) -> Self:
137 """
138 Create an Attitude instance from a rotation matrix (or direction cosine
139 matrix), defined such that it transforms vectors from the body frame, {b},
140 to the navigation frame, {n}, using:
142 v_n = A @ v_b
144 where,
145 - ``A`` is the 3x3 rotation matrix (or direction cosine matrix).
146 - v_b is a vector expressed in the body frame, {b}.
147 - v_n is the same vector expressed in the navigation frame, {n}.
149 Parameters
150 ----------
151 A : ArrayLike
152 Rotation matrix (element of SO(3)).
154 Returns
155 -------
156 Attitude
157 Attitude instance.
158 """
159 A = _asarray_check_matrix_so3(A)
160 q = _quaternion_from_matrix(A)
161 return cls(q)
163 def as_matrix(self) -> NDArray[np.float64]:
164 """
165 Return the attitude as a rotation matrix (or direction cosine matrix), A,
166 defined such that it transforms vectors from the body frame, {b}, to the
167 navigation frame, {n}, using:
169 v_n = A @ v_b
171 where,
173 - ``A`` is the 3x3 attitude matrix.
174 - v_b is a vector expressed in the body frame, {b}.
175 - v_n is the same vector expressed in the navigation frame, {n}.
177 The rotation matrix is computed from the unit quaternion, q, using the formula:
179 A = I + 2 * q_w * S(q_xyz) + 2 * S(q_xyz)^2
181 where,
183 - I is the 3x3 identity matrix.
184 - q_w is the scalar part of the unit quaternion.
185 - q_xyz is the vector part, [q_x, q_y, q_z], of the unit quaternion.
186 - S(q_xyz) is the skew-symmetric matrix of q_xyz.
188 Returns
189 -------
190 numpy.ndarray, shape (3, 3)
191 Rotation matrix (or direction cosine matrix).
192 """
193 return _rot_matrix_from_quaternion(self._q)
195 @classmethod
196 def from_euler(cls, theta: ArrayLike, degrees: bool = False) -> Self:
197 """
198 Create an Attitude instance from a set of Euler angles (ZYX convention)
199 (see Notes).
201 Parameters
202 ----------
203 theta : ArrayLike
204 Set of three Euler angles (ZYX convention), [alpha, beta, gamma], representing
205 rotations about the X, Y, and Z axes, respectively.
206 degrees : bool, default False
207 If True, the input angles are interpreted as degrees. Otherwise, they are
208 interpreted as radians. Internally, angles are stored as radians.
210 Returns
211 -------
212 Attitude
213 Attitude instance.
215 Notes
216 -----
217 The ZYX Euler angles describe how to transition from the 'navigation' frame
218 to the 'body' frame through three consecutive intrinsic and passive rotations
219 in the ZYX order.
221 Defined as:
223 A = R_z(gamma) @ R_y(beta) @ R_x(alpha)
225 where,
227 - gamma is a first rotation about the navigation frame's Z-axis.
228 - beta is a second rotation about the intermediate Y-axis.
229 - alpha is a final rotation about the second intermediate X-axis to arrive
230 at the body frame.
232 and A is the attitude matrix (transforming vectors from the body frame to
233 the navigation frame):
235 v_n = A @ v_b
236 """
237 theta = np.asarray_chkfinite(theta, dtype=float).reshape(3)
238 if degrees:
239 theta = np.radians(theta)
240 q = _quaternion_from_euler_zyx(theta)
241 return cls(q)
243 def as_euler(self, degrees: bool = False) -> NDArray[np.float64]:
244 """
245 Convert the attitude to (ZYX) Euler angles (see Notes).
247 Parameters
248 ----------
249 degrees : bool, default False
250 If True, the output angles are in degrees. Otherwise, they are in radians.
252 Returns
253 -------
254 numpy.ndarray, shape (3,)
255 The 3-element Euler (ZYX) angles, [alpha, beta, gamma], representing
256 rotations about the X, Y, and Z axes, respectively.
258 Notes
259 -----
260 The ZYX Euler angles describe how to transition from the 'navigation' frame
261 to the 'body' frame through three consecutive intrinsic and passive rotations
262 in the ZYX order.
264 Defined as:
266 A = R_z(gamma) @ R_y(beta) @ R_x(alpha)
268 where,
270 - gamma is a first rotation about the navigation frame's Z-axis.
271 - beta is a second rotation about the intermediate Y-axis.
272 - alpha is a final rotation about the second intermediate X-axis to arrive
273 at the body frame.
275 and A is the attitude matrix (transforming vectors from the body frame to
276 the navigation frame):
278 v_n = A @ v_b
279 """
280 theta = _euler_zyx_from_quaternion(self._q)
281 if degrees:
282 theta = np.degrees(theta)
283 return theta