AHRS 工具函数 (tools)

pywayne.ahrs.tools.quaternion_decompose(quaternion: ndarray) → Tuple[ndarray, ndarray, ndarray]

This function is used to decompose a quaternion into a rotation angle around vertical axis (heading) and a rotation angle around horizontal axis (inclination).

\[\begin{split}\begin{array}{rcl} \theta &=& 2\arccos{q_w} \\ \theta_h &=& 2\arctan{|\frac{q_z}{q_w}|} \\ \theta_i &=& 2\arccos{\sqrt{q_w^2+q_z^2}} \end{array}\end{split}\]

Parameters:

quaternion (np.ndarray) – shape: (4,) or (1, 4) or (N, 4)

Returns:

angle (np.ndarray) – the angle corresponds to the quaternion
angle_heading (np.ndarray) – the angle around vertical axis
angle_inclination (np.ndarray) – the angle around horizontal axis

Examples

>>> from pywayne.ahrs.tools import quaternion_decompose
>>> q = np.array([np.sqrt(2) / 2, np.sqrt(2) / 2, 0, 0])
>>> print(quaternion_decompose(q))
(array([1.57079633]), array([0.]), array([1.57079633]))

pywayne.ahrs.tools.quaternion_roll_pitch_compensate(quaternion: ndarray) → ndarray

This function is used to calculate compensation rotation to make the orientation’s pitch & roll to zero.

Here is how to conduct the formula,

\[\begin{split}\begin{array}{rcl} \textbf{R}_{cur} &=& (\textbf{R}_z\textbf{R}_y\textbf{R}_x)\textbf{R}_{identity} \\ &=& \textbf{R}_z\textbf{R}_y\textbf{R}_x \\ \textbf{R}_{corr} &=& \textbf{R}_y\textbf{R}_x=\textbf{R}_z^{-1}\textbf{R}_{cur} \\ \textbf{R}_{corrected} &=& \textbf{R}_{ori}\textbf{R}_{corr} \\ &=& \textbf{R}_{ori}\textbf{R}_z^{-1}\textbf{R}_{cur} \end{array}\end{split}\]

Here is how to use the compensation rotation,

\[\begin{array}{rcl} \textbf{R}_{cur}\textbf{R}_{corr}=\textbf{R}_{z}\textbf{R}_y\textbf{R}_x(\textbf{R}_y\textbf{R}_x)^{-1}=\textbf{R}_z \end{array}\]

Parameters:: quaternion (np.ndarray) – shape: (4,) or (1, 4) or (4, 1)
Returns:: q_yx_inv – compensation rotation
Return type:: np.ndarray

Examples

>>> from pywayne.ahrs.tools import quaternion_roll_pitch_compensate
>>> q = np.array([0.989893, -0.099295, 0.024504, -0.098242])
>>> print(quaternion_roll_pitch_compensate(q))
[ 0.99475519  0.10125102 -0.01442838 -0.00146859]

本模块提供了用于姿态估计的工具函数，主要基于四元数数学原理来解算物体的整体旋转角、航向角及倾斜角。模块中主要包含两个函数，分别用于四元数分解和姿态补偿。

pywayne.ahrs.tools.quaternion_decompose(quaternion: ndarray) → Tuple[ndarray, ndarray, ndarray]

This function is used to decompose a quaternion into a rotation angle around vertical axis (heading) and a rotation angle around horizontal axis (inclination).

\[\begin{split}\begin{array}{rcl} \theta &=& 2\arccos{q_w} \\ \theta_h &=& 2\arctan{|\frac{q_z}{q_w}|} \\ \theta_i &=& 2\arccos{\sqrt{q_w^2+q_z^2}} \end{array}\end{split}\]

Parameters:

quaternion (np.ndarray) – shape: (4,) or (1, 4) or (N, 4)

Returns:

angle (np.ndarray) – the angle corresponds to the quaternion
angle_heading (np.ndarray) – the angle around vertical axis
angle_inclination (np.ndarray) – the angle around horizontal axis

Examples

>>> from pywayne.ahrs.tools import quaternion_decompose
>>> q = np.array([np.sqrt(2) / 2, np.sqrt(2) / 2, 0, 0])
>>> print(quaternion_decompose(q))
(array([1.57079633]), array([0.]), array([1.57079633]))

示例:

>>> from pywayne.ahrs.tools import quaternion_decompose
>>> import numpy as np
>>> q = np.array([np.sqrt(2)/2, np.sqrt(2)/2, 0, 0])
>>> print(quaternion_decompose(q))

pywayne.ahrs.tools.quaternion_roll_pitch_compensate(quaternion: ndarray) → ndarray

This function is used to calculate compensation rotation to make the orientation’s pitch & roll to zero.

Here is how to conduct the formula,

\[\begin{split}\begin{array}{rcl} \textbf{R}_{cur} &=& (\textbf{R}_z\textbf{R}_y\textbf{R}_x)\textbf{R}_{identity} \\ &=& \textbf{R}_z\textbf{R}_y\textbf{R}_x \\ \textbf{R}_{corr} &=& \textbf{R}_y\textbf{R}_x=\textbf{R}_z^{-1}\textbf{R}_{cur} \\ \textbf{R}_{corrected} &=& \textbf{R}_{ori}\textbf{R}_{corr} \\ &=& \textbf{R}_{ori}\textbf{R}_z^{-1}\textbf{R}_{cur} \end{array}\end{split}\]

Here is how to use the compensation rotation,

\[\begin{array}{rcl} \textbf{R}_{cur}\textbf{R}_{corr}=\textbf{R}_{z}\textbf{R}_y\textbf{R}_x(\textbf{R}_y\textbf{R}_x)^{-1}=\textbf{R}_z \end{array}\]

Parameters:: quaternion (np.ndarray) – shape: (4,) or (1, 4) or (4, 1)
Returns:: q_yx_inv – compensation rotation
Return type:: np.ndarray

Examples

>>> from pywayne.ahrs.tools import quaternion_roll_pitch_compensate
>>> q = np.array([0.989893, -0.099295, 0.024504, -0.098242])
>>> print(quaternion_roll_pitch_compensate(q))
[ 0.99475519  0.10125102 -0.01442838 -0.00146859]

示例:

>>> from pywayne.ahrs.tools import quaternion_roll_pitch_compensate
>>> import numpy as np
>>> q = np.array([0.989893, -0.099295, 0.024504, -0.098242])
>>> print(quaternion_roll_pitch_compensate(q))