Algebraic Quaternion Algorithm¶
Roberto Valenti’s Algebraic Quaterion Algorithm (AQUA) [Valenti2015] estimates a quaternion with the algebraic solution of a system from inertial/magnetic observations, solving Wahba’s Problem.
AQUA computes the “tilt” quaternion and the “heading” quaternion separately in two subparts. This avoids the impact of the magnetic disturbances on the roll and pitch components of the orientation.
AQUA can be used with a complementary filter to fuse the gyroscope data together with accelerometer and magnetic field readings. The correction part of the filter is based on the independently estimated quaternions and works for both IMU (Inertial Measurement Unit) and MARG (Magnetic, Angular Rate, and Gravity) sensors [Valenti2016].
Quaternion as Orientation¶
Any orientation in a threedimensional euclidean space of a frame \(A\) with respect to a frame \(B\) can be represented by a unit quaternion (a.k.a. versor), \(\mathbf{q}\in\mathbb{H}^4\), in Hamiltonian space defined as:
where \(\alpha\) is the rotation angle and \(\mathbf{e}=\begin{bmatrix}e_x&e_y&e_z\end{bmatrix}^T\) is the unit vector representing the rotation axis. The conjugate quaternion is used to represent the orientation of frame \(B\) relative to frame \(A\):
The final orientation quaternion can be easily found through quaternion multiplication:
where the quaternion multiplication of any given quaternions \(\mathbf{p}\) and \(\mathbf{q}\) is computed as:
Unit quaternions in Hamiltonian spaces are often used to operate rotations of vectors in a 3D euclidean space. A vector \(^A\mathbf{v}=\begin{bmatrix}v_x & v_y & v_z\end{bmatrix}^T\) expressed with respect to frame \(A\) can be represented with respect to frame \(B\) as:
where the vector \(^A\mathbf{v}\) is rewritten as the pure quaternion \(^A\mathbf{v}_q=\begin{bmatrix}0 & v_x & v_y & v_z\end{bmatrix}^T\), so that it can multiply with the versor and its conjugate.
The inverse rotation can be achieved by flipping the versors:
Quaternion as Rotation Matrix in SO(3)
These rotations can also be expressed within the threedimensional euclidean space if we express the rotation as a Direction Cosine Matrix \(\mathbf{R}\in SO(3)\):
And, because the Direction Cosine Matrix belongs to the Special Orthogonal Group \(SO(3)\), the inverse rotation is simply its transpose:
Note
For a more detailed explanation of quaternions and their use in spatial rotation see the documentation of the class Quaternions.
Given two quaternions \(\mathbf{p}\) and \(\mathbf{q}\), the cosine of the angle \(\Omega\) subtended by the arc between them is equal to the dot product of the two quaternions.
It is easy to see that the dot product of any \(\mathbf{q}=\begin{bmatrix}q_w & q_x & q_y & q_z\end{bmatrix}\) and the identity quaternion \(\mathbf{q}_I=\begin{bmatrix}1 & 0 & 0 & 0\end{bmatrix}\) is equal to the \(q_w\) component:
Interpolations
The simple Linear intERPolation (LERP) between two quaternions \(\mathbf{p}\) and \(\mathbf{q}\) is obtained as:
where \(\alpha\in [0, 1]\). But this does not keep the unit norm, so we must normalize the resulting interpolation:
The Spherical Linear intERPolation (SLERP) gives a correct evaluation of the weighted average of two points lying on a curve. In the case of quaternions, the points lie on the surface of the 4D sphere (hypersphere).
Quaternion from EarthField Observations¶
The local (sensor) frame is labeled as \(L\), and the global (Earth) frame as \(G\). The measured acceleration, \(^L\mathbf{a}\), and the true Earth gravitational acceleration, \(^G\mathbf{g}\), are defined as unit vectors [1]:
The measured local magnetic field, \(^L\mathbf{m}\), and the true geomagnetic field, \(^G\mathbf{h}\), are also unit vectors:
The gyroscopes measure the angular velocity, \(^L\mathbf{\omega}\), around the three sensor frame axes:
The measured angular velocities are not normalized, unlike the other sensors, and are assumed to be in radians per second.
A straightforward way to formulate the quaternion, \(^L_G\mathbf{q}\), relating the global frame \(G\) to the local frame \(L\), is through the inverse orientation which rotates the measured quantities \(^L\mathbf{a}\) and \(^L\mathbf{m}\) into the reference quantities \(^G\mathbf{g}\) and \(^G\mathbf{h}\):
In the case of a disagreement between the gravitational and magnetometer readings, the system will not have a solution. To address this problem a modified equation system is built.
First, the global coordinate frame \(G\) is aligned with the magnetic North. The global frame’s Xaxis points to the same direction as the local magnetic field (Zaxis remains vertical.)
Warning
This global frame is only fixed in case the local magnetic field does not change its heading. Thus, no magnetic inference should be present.
Let \(^G\Pi_{zx^+}\) be the halfplane which contains all points that lie in the global XZplane such that X is nonnegative.
The magnetic reading, when rotated into the global frame, must lie on the halfplane \(^G\Pi_{zx^+}\) to guarantee that the heading will be measured with respect to the geomagnetic North.
This way we don’t need apriori knowledge of the direction of Earth’s magnetic field \(^G\mathbf{h}\).
The orientation \(^L_G\mathbf{q}\) is decomposed into two auxiliary quaternions, such that:
The quaternion \(\mathbf{q}_{\mathrm{mag}}\) represents a rotation around the Zaxis to point North only:
Quaternion from Accelerometer¶
The auxiliary quaternion \(\mathbf{q}_\mathrm{acc}\) is built as a function of \(^L\mathbf{a}\). The observations of the gravity vector in the two reference frames allows us to find the quaternion that performs the transformation between the two representations.
The representation of the gravity vector in the global frame \(G\) only has a component on the Zaxis. Thus, any rotation about this axis does not produce any change on it.
The alignment of the gravity vector from global frame into local frame can be achieved by infinite rotations with definite roll and pitch angles and arbitrary yaw. To restrict the solutions to a finite number \(q_{z\mathrm{acc}}=0\) is chosen.
This gives four solutions of \(\mathbf{q}_\mathrm{acc}\). Two are discarded for having a negative norm, and from the remaining two the one with a positive \(q_w\) is taken.
But it has a singularity at \(a_z=1\). Therefore, the final solution has to be defined as:
Quaternion from Magnetometer¶
The auxiliary quaternion \(\mathbf{q}_\mathrm{mag}\) is derived as a function of \(^L\mathbf{m}\) and \(\mathbf{q}_\mathrm{acc}\).
The quaternion \(\mathbf{q}_\mathrm{acc}\) is used to rotate the magnetic field vector \(^L\mathbf{m}\) into an intermediate frame whoe Zaxis is the same as the global coordinate frame with X and Yaxes pointing to unknown directions due to the unknown yaw of \(\mathbf{q}_\mathrm{acc}\).
where \(\mathbf{l}\) is the rotated magnetic field vector. Then, we find the quaternion :math:mathbf{q}_mathrm{mag} that rotates the vector \(\mathbf{l}\) into the vector that lies on \(^G\Pi_{zx^+}\):
where \(\Gamma=l_x^2+l_y^2\). This quaternion performs a rotation only about the global Zaxis to align the Xaxis of the intermediate frame with the Xaxis of the global frame without affecting the roll and pitch. Therefore, if there is a magnetic interference, it would affect only the headin angle.
The solution to find this quaternion ensuring the shortest rotation is:
This quaternion has a singularity too, but here it happens when \(l_x<0\) and \(l_y=0\). Eventually, a simliar solution is found restraining the condition of \(l_x\):
The generalized quaternion orientation of the global frame relative to the local frame as the mulitplication of two quaternions \(\mathbf{q}_\mathrm{acc}\) and \(\mathbf{q}_\mathrm{mag}\):
The quaternion \(^L_G\mathbf{q}\) does not suffer from the discontinuity problem of the yaw angle given by the switching formulation of \(\mathbf{q}_\mathrm{acc}\) thanks to the multiplication with \(\mathbf{q}_\mathrm{mag}\), which performs the alignment of the intermediate frame into the global frame.
QuaternionBased Complementary Filter¶
A complementary filter fuses attitude estimation in quaternion form from gyroscope data with accelerometer and magnetometer data in the form of a delta quaternion.
If only IMU data is provided (gyroscopes and accelerometers), it corrects only roll and pitch of the attitude. If magnetometer data is also provided (MARG) a second step is added to the algorithm where a magnetic delta quaternion is derived to correct the heading of the previous estimation by aligning the current frame with the magnetic field.
In the Prediction step the measured angular velocity is used to compute a first estimation of the orientation in quaternion form.
Most literature estimating the quaternion derivative from an angular rate measurement is usually calculated for the one representing the orientation of the local frame with respect to the global frame.
However, Valenti uses the inverse orientation, so the quaternion derivative is computed using the inverse unit quaternion, which is simply the conjugate:
where \(^L\mathbf{\omega}_{q, t_k}=\begin{bmatrix}0 & \omega_x & \omega_y & \omega_z\end{bmatrix}^T\) is the measured angular velocity, in radians per second, arranged as a pure quaternion at time \(t_k\), and \(^L_G\mathbf{q}_{t_{k1}}\) is the previous estimate of the orientation.
The orientation of the global frame relative to local frame at time \(t_k\) can be finally computed by numerically integrating the quaternion derivative using the sampling period \(\Delta t = t_k  t_{k1}\).
The Correction step is based on a multiplicative approach, where the predicted quaternion \(^L_G\mathbf{q}_\omega\) is corrected by means of two delta quaternions:
The delta quaternions are computed and filtered independently by the highfrequency noise. This correction is divided in two steps: correction of roll and pitch of the predicted quaternion, and then the correction of the yaw angle if readings of the magnetic field are provided.
AccelerometerBased Correction¶
The inverse predicted quaternion \(^G_L\mathbf{q}_\omega\) is used to rotate the normalized body frame gravity vector \(^L\mathbf{a}\), measured by the accelerometer, into the global frame:
where \(^G\mathbf{g}_p=\begin{bmatrix}g_x & g_y & g_z\end{bmatrix}^T\) is the predicted gravity, which always has a small deviation from the real gravity vector \(^G\mathbf{g}=\begin{bmatrix}0 & 0 & 1\end{bmatrix}^T\). We compute the delta quaternion \(\Delta\mathbf{q}_\mathrm{acc}\) to rotate \(^G\mathbf{g}\) into \(^G\mathbf{g}_p\):
Similar to the auxiliary quaternions, we find a closedform solution:
This has a singularity at \(g_z=1\), but it can be ignored, because the value of \(g_z\) will always be closer to 1.
The delta quaternion is affected by the accelerometer’s high frequency noise, so we scale it down by using an interpolation with the identity quaternion \(\mathbf{q}_I\). As demonstrated above, the dot product with \(\mathbf{q}_I\) is equal to the \(\Delta q_{w\mathrm{acc}}\) component of \(\Delta\mathbf{q}_\mathrm{acc}\).
If \(\Delta q_{w\mathrm{acc}}>\epsilon\), where \(\epsilon\) is a threshold value (default is \(\epsilon=0.9\)), a simple LERP is used:
The predicted quaternion from gyroscopes is multiplied with the delta quaternion to correct the roll and pitch components:
The heading angle predicted by the gyroscope integration is not corrected in this step.
MagnetometerBased Correction¶
When a magnetic field measurement is provided the second step corrects the heading component. We use the quaternion inverse of \(^L_G\mathbf{q}'\) to rotate the magnetic field vector \(^L\mathbf{m}\) from the body frame into the world frame.
The delta quaternion \(\Delta\mathbf{q}_\mathrm{mag}\) rotates the vector \(\mathbf{l}\) into the vector that lies on the XZsemiplane:
The solution to the above ensures the shortest rotation:
This delta quaternion is affected by the noise of the magnetometer, which is also filtered like the \(\Delta\mathbf{q}_\mathrm{acc}\) switching between LERP and SLERP according to the same criterion.
Because each delta quaternion is affected independently with different noises, two different thresholds can be used: \(\alpha\) for the accelerometer and \(\beta\) for the magnetometer to obtain \(\widehat{\Delta\mathbf{q}}_\mathrm{mag}\).
Finally, the delta quaternion is multiplied with \(^L_G\mathbf{q}'\) to obtain the orientation of the global frame with respect to the local frame:
Adaptive Gain¶
When the vehicle moves with high acceleration, the magnitude and direction of the total measured acceleration vector are different from gravity, and the attitude is evaluated using a false reference.
However, the gyroscope readings are not affected by linear acceleration, thus they can still be used to compute a relatively accurate orientation estimation.
A constant gain fusion algorithm cannot overcome the aforementioned problem if the optimal gain has been evaluated for static conditions. An adaptive gain can bw used to tackle this problem.
First a magnitude error \(e_m\) is defined:
where \(\\,^L\hat{a}\\) is the norm of the measured local frame acceleration vector before normalization and \(g=9.81 \, \frac{m}{s^2}\).
From the LERP and SLERP definitions, we make the filtering gain \(\alpha\) dependent on the magnitude error \(e_m\) through the gain factor \(f\):
where \(\overline{\alpha}\) is the constant value that gives the best filtering result in static conditions and \(f(e_m)\) is what is called the gain factor, which is a piecewise continuous function of the magnitude error.
This gain factor is equal to \(1\) when the magnitude of the nongravitational acceleration is not high enough to overcome the acceleration gravity and the value of the error magnitude does not reach the first threshold \(t_1\). If the nongravitational acceleration rises and the error magnitude exceeds that first threshold, the gain factor decreases linearly with the increase of the magnitude error until reaching zero for error magnitude equal to the second threshold \(t_2\) and over.
Empirically, the threshold values giving the best results are \(0.1\) and \(0.2\).
Filter Initialization¶
The values of the current bodyframe acceleration and magnetic field vectors are used to produce the quaternion representing the initial orientation of the rigid body in any configuration. Therefore, for the initialization, the filter does not need any assumption and it is performed in one single step.
The bias of a gyroscope’s reading is a slowvarying signal considered as low frequency noise. A lowpass filtering is applied to separate the bias from the actual angular velocity, but only when the sensor is in a steadystate condition to avoid filtering useful information.
If the sensor is in the steadystate condition, the bias is updated, otherwise it is assumed to be equal to the previous step value. The estimated bias is then subtracted from the gyroscope reading obtaining a biasfree angular velocity measurement.
Footnotes¶
[1]  Any vector \(\mathbf{x}\) is a unit vector if \(\\mathbf{x}\=1\). 
References
[Valenti2015]  Valenti, R.G.; Dryanovski, I.; Xiao, J. Keeping a Good Attitude: A QuaternionBased Orientation Filter for IMUs and MARGs. Sensors 2015, 15, 1930219330. (https://res.mdpi.com/sensors/sensors1519302/article_deploy/sensors1519302.pdf) 
[Valenti2016]  R. G. Valenti, I. Dryanovski and J. Xiao, “A Linear Kalman Filter for MARG Orientation Estimation Using the Algebraic Quaternion Algorithm,” in IEEE Transactions on Instrumentation and Measurement, vol. 65, no. 2, pp. 467481, 2016. (https://ieeexplore.ieee.org/document/7345567) 

class
ahrs.filters.aqua.
AQUA
(acc: numpy.ndarray = None, mag: numpy.ndarray = None, gyr: numpy.ndarray = None, **kw)¶ Algebraic Quaternion Algorithm
Parameters:  gyr (numpy.ndarray, default: None) – Nby3 array with measurements of angular velocity in rad/s
 acc (numpy.ndarray, default: None) – Nby3 array with measurements of acceleration in m/s^2
 mag (numpy.ndarray, default: None) – Nby3 array with measurements of magnetic field in nT
 frequency (float, default: 100.0) – Sampling frequency in Herz
 Dt (float, default: 0.01) – Sampling step in seconds. Inverse of sampling frequency. Not required
if
frequency
value is given.  alpha (float, default: 0.01) – Gain characterizing cutoff frequency for accelerometer quaternion
 beta (float, default: 0.01) – Gain characterizing cutoff frequency for magnetometer quaternion
 threshold (float, default: 0.9) – Threshold to discriminate between LERP and SLERP interpolation
 adaptive (bool, default: False) – Whether to use an adaptive gain for each sample
 q0 (numpy.ndarray, default: None) – Initial orientation, as a versor (normalized quaternion).
Variables:  gyr (numpy.ndarray) – Nby3 array with N gyroscope samples.
 acc (numpy.ndarray) – Nby3 array with N accelerometer samples.
 mag (numpy.ndarray) – Nby3 array with N magnetometer samples.
 frequency (float) – Sampling frequency in Herz
 Dt (float) – Sampling step in seconds. Inverse of sampling frequency.
 alpha (float) – Gain characterizing cutoff frequency for accelerometer quaternion.
 beta (float) – Gain characterizing cutoff frequency for magnetometer quaternion.
 threshold (float) – Threshold to discern between LERP and SLERP interpolation.
 adaptive (bool) – Flag indicating use of adaptive gain.
 q0 (numpy.ndarray) – Initial orientation, as a versor (normalized quaternion).

Omega
(x: numpy.ndarray) → numpy.ndarray¶ Omega operator.
Given a vector \(\mathbf{x}\in\mathbb{R}^3\), return a \(4\times 4\) matrix of the form:
\[\begin{split}\boldsymbol\Omega(\mathbf{x}) = \begin{bmatrix} 0 & \mathbf{x}^T \\ \mathbf{x} & \lfloor\mathbf{x}\rfloor_\times \end{bmatrix} = \begin{bmatrix} 0 & x_1 & x_2 & x_3 \\ x_1 & 0 & x_3 & x_2 \\ x_2 & x_3 & 0 & x_1 \\ x_3 & x_2 & x_1 & 0 \end{bmatrix}\end{split}\]This operator is a simplification to create a 4by4 matrix used for the product between the angular rate and a quaternion, such that:
\[^L_G\dot{\mathbf{q}}_{\omega, t_k} = \frac{1}{2}\boldsymbol\Omega(\,^L\mathbf{\omega}_{q, t_k})\;^L_G\mathbf{q}_{t_{k1}}\]Note
The original definition in the article (eq. 39) has an errata missing the multiplication with \(\frac{1}{2}\).
Parameters: x (numpy.ndarray) – Threedimensional vector representing the angular rate around the three axes of the local frame. Returns: Omega – Omega matrix. Return type: numpy.ndarray

estimate
(acc: numpy.ndarray, mag: numpy.ndarray = None) → numpy.ndarray¶ Quaternion from EarthField Observations
Algebraic estimation of a quaternion as a function of an observation of the Earth’s gravitational force and magnetic fields.
It decomposes the quaternion \(\mathbf{q}\) into two auxiliary quaternions \(\mathbf{q}_{\mathrm{acc}}\) and \(\mathbf{q}_{\mathrm{mag}}\), such that:
\[\mathbf{q} = \mathbf{q}_{\mathrm{acc}}\mathbf{q}_{\mathrm{mag}}\]Parameters:  acc (numpy.ndarray) – Sample of triaxial Accelerometer in m/s^2
 mag (numpy.ndarray, default: None) – Sample of triaxial Magnetometer in mT
Returns: q – Estimated quaternion.
Return type: numpy.ndarray

init_q
(acc: numpy.ndarray, mag: numpy.ndarray = None) → numpy.ndarray¶ Synonym of method estimate.

updateIMU
(q: numpy.ndarray, gyr: numpy.ndarray, acc: numpy.ndarray, dt: float = None) → numpy.ndarray¶ Quaternion Estimation with a IMU architecture.
The estimation is made in two steps: a prediction is done with the angular rate (gyroscope) to integrate and estimate the current orientation; then a correction step uses the measured accelerometer to infer the expected gravity vector and use it to correct the predicted quaternion.
If the gyroscope data is invalid, it returns the given apriori quaternion. Secondly, if the accelerometer data is invalid the predicted quaternion (using gyroscopes) is returned.
Parameters:  q (numpy.ndarray) – Apriori quaternion.
 gyr (numpy.ndarray) – Sample of triaxial Gyroscope in rad/s.
 acc (numpy.ndarray) – Sample of triaxial Accelerometer in m/s^2
 dt (float, default: None) – Time step, in seconds, between consecutive Quaternions.
Returns: q – Estimated quaternion.
Return type: numpy.ndarray

updateMARG
(q: numpy.ndarray, gyr: numpy.ndarray, acc: numpy.ndarray, mag: numpy.ndarray, dt: float = None) → numpy.ndarray¶ Quaternion Estimation with a MARG architecture.
The estimation is made in two steps: a prediction is done with the angular rate (gyroscope) to integrate and estimate the current orientation; then a correction step uses the measured accelerometer and magnetic field to infer the expected geodetic values. Its divergence is used to correct the predicted quaternion.
If the gyroscope data is invalid, it returns the given apriori quaternion. Secondly, if the accelerometer data is invalid the predicted quaternion (using gyroscopes) is returned. Finally, if the magnetometer measurements are invalid, returns a quaternion corrected by the accelerometer only.
Parameters:  q (numpy.ndarray) – Apriori quaternion.
 gyr (numpy.ndarray) – Sample of triaxial Gyroscope in rad/s.
 acc (numpy.ndarray) – Sample of triaxial Accelerometer in m/s^2
 mag (numpy.ndarray) – Sample of triaxial Magnetometer in mT
 dt (float, default: None) – Time step, in seconds, between consecutive Quaternions.
Returns: q – Estimated quaternion.
Return type: numpy.ndarray

ahrs.filters.aqua.
adaptive_gain
(gain: float, a_local: numpy.ndarray, t1: float = 0.1, t2: float = 0.2, g: float = 9.809030668031474) → float¶ Adaptive filter gain factor
The estimated gain \(\alpha\) is dependent on the gain factor \(f(e_m)\):
\[\alpha = a f(e_m)\]where the magnitude error is defined by the measured acceleration \(\mathbf{a}\) before normalization and the reference gravity \(g\approx 9.809196 \, \frac{m}{s^2}\):
\[e_m = \frac{\\mathbf{a}\g}{g}\]The gain factor is constant and equal to 1 when the magnitude of the nongravitational acceleration is not high enough to overcome gravity.
If nongravitational acceleration rises and \(e_m\) exceeds the first threshold, the gain factor \(f\) decreases linearly with the increase of the magnitude until reaching zero at the second threshold and above it.
\[\begin{split}f(e_m) = \left\{ \begin{array}{ll} 1 & \mathrm{if}\; e_m \leq t_1 \\ \frac{t_2e_m}{t_1} & \mathrm{if}\; t_1 < e_m < t_2 \\ 0 & \mathrm{if}\; e_m \geq t_2 \end{array} \right.\end{split}\]Empirically, both thresholds have been defined at
0.1
and0.2
, respectively. They can be, however, changed by setting the values of input parameterst1
andt2
.Parameters:  gain (float) – Gain yielding best results in static conditions.
 a_local (numpy.ndarray) – Measured local acceleration vector.
 t1 (float, default: 0.1) – First threshold.
 t2 (float, default: 0.2) – Second threshold.
 g (float, default: 9.809196) – Reference gravitational acceleration in m/s^2. The estimated gravity in
Munich, Germany (
9.809196
) is used as default reference value.
Returns: alpha – Gain factor.
Return type: float
Examples
>>> from ahrs.filters.aqua import adaptive_gain >>> alpha = 0.01 # Best gain in static conditions >>> acc = np.array([0.0699, 9.7688, 0.2589]) # Measured acceleration. Quasistatic state. >>> adaptive_gain(alpha, acc) 0.01 >>> acc = np.array([0.8868, 10.8803, 0.4562]) # New measured acceleration. Slightly above first threshold. >>> adaptive_gain(alpha, acc) 0.008615664547367627 >>> acc = np.array([4.0892, 12.7667, 2.6047]) # New measured acceleration. Above second threshold. >>> adaptive_gain(alpha, acc) 0.0 >>> adaptive_gain(alpha, acc, t1=0.2, t2=0.5) # Same acceleration. New thresholds. 0.005390131074499384 >>> adaptive_gain(alpha, acc, t1=0.2, t2=0.5, g=9.82) # Same acceleration and thresholds. New reference gravity. 0.005466716107480152

ahrs.filters.aqua.
slerp_I
(q: numpy.ndarray, ratio: float, t: float) → numpy.ndarray¶ Interpolation with identity quaternion
Interpolate a given quaternion with the identity quaternion \(\mathbf{q}_I=\begin{pmatrix}1 & 0 & 0 & 0\end{pmatrix}\) to scale it to closest versor.
The interpolation can be with either LERP (Linear) or SLERP (Spherical Linear) methods, decided by a threshold value \(t\), which lies between
0.0
and1.0
.\[\begin{split}\mathrm{method} = \left\{ \begin{array}{ll} \mathrm{LERP} & \: q_w > t \\ \mathrm{SLERP} & \: \mathrm{otherwise} \end{array} \right.\end{split}\]For LERP, a simple equation is implemented:
\[\hat{\mathbf{q}} = (1\alpha)\mathbf{q}_I + \alpha\Delta \mathbf{q}\]where \(\alpha\in [0, 1]\) is the gain characterizing the cutoff frequency of the filter. It basically decides how “close” to the given quaternion or to the identity quaternion the interpolation is.
If the scalar part \(q_w\) of the given quaternion is below the threshold \(t\), SLERP is used:
\[\hat{\mathbf{q}} = \frac{\sin([1\alpha]\Omega)}{\sin\Omega} \mathbf{q}_I + \frac{\sin(\alpha\Omega)}{\sin\Omega} \mathbf{q}\]where \(\Omega=\arccos(q_w)\) is the subtended arc between the quaternions.
Parameters:  q (numpy.array) – Quaternion to inerpolate with.
 ratio (float) – Gain characterizing the cutoff frequency of the filter.
 t (float) – Threshold deciding interpolation method. LERP when qw>t, otherwise SLERP.
Returns: q – Interpolated quaternion
Return type: numpy.array