SLERP#

ahrs.common.quaternion.slerp(p: ndarray, q: ndarray, t_array: ndarray, threshold: float = 0.9995) → ndarray#

Spherical Linear Interpolation between two quaternions.

A SLERP to find a single quaternion \(\mathbf{r}\) between two quaternions \(\mathbf{p}\) and \(\mathbf{q}\) is defined as:

\[\mathbf{r} = \frac{\sin([1-\alpha]\Omega)}{\sin\Omega}\mathbf{p} + \frac{\sin(\alpha\Omega)}{\sin\Omega}\mathbf{q}\]

where \(\alpha\) is a weight in the interval [0, 1] and \(\Omega\) is the angle between the two quaternions. This is a great circle interpolation, where the shortest path between two points on a sphere is followed.

This function returns an array of quaternion orientations along the minor arc of the great circle passing through quaternion endpoints \(\mathbf{p}\) and \(\mathbf{q}\) lying on the unit radius hypersphere.

It returns as many orientations between p and q as elements in t_array, which must be an array of weights in the interval [0, 1].

Based on the method detailed in [Wikd].

If the dot product between the two quaternions is greater than the threshold, the interpolation is done linearly (LERP). Otherwise, the interpolation is done spherically (SLERP).

Parameters:

  • p (numpy.ndarray) – First endpoint quaternion.

  • q (numpy.ndarray) – Second endpoint quaternion.

  • t_array (numpy.ndarray) – Array of weights to interpolate to. Values closer to 0 are closer to first quaternion, and values closer to 1 are closer to second quaternion.

  • threshold (float, default: 0.9995) – Threshold to closeness of interpolation.

Returns:

q – New array of quaternions representing the interpolated orientations.

Return type:

numpy.ndarray

Example

Create a single quaternion exactly in the middle of the two quaternions:

>>> slerp(p, q, [0.5])
array([[0.70710678, 0.70710678, 0.        , 0.        ]])

Create 5 equidistant orientations between quaternions \(\mathbf{p} = \begin{bmatrix}1 & 0 & 0 & 0\end{bmatrix}\) and \(\mathbf{q} = \begin{bmatrix}0 & 1 & 0 & 0\end{bmatrix}\):

>>> p = [1.0, 0.0, 0.0, 0.0]
>>> q = [0.0, 1.0, 0.0, 0.0]
>>> slerp(p, q, [0.0, 0.2, 0.4, 0.6, 0.8, 1.0])
array([[1.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],
       [9.51056516e-01, 3.09016994e-01, 0.00000000e+00, 0.00000000e+00],
       [8.09016994e-01, 5.87785252e-01, 0.00000000e+00, 0.00000000e+00],
       [5.87785252e-01, 8.09016994e-01, 0.00000000e+00, 0.00000000e+00],
       [3.09016994e-01, 9.51056516e-01, 0.00000000e+00, 0.00000000e+00],
       [6.12323400e-17, 1.00000000e+00, 0.00000000e+00, 0.00000000e+00]])
On this page

This Page