Complementary Filter¶

Attitude quaternion obtained with gyroscope and accelerometer-magnetometer measurements, via complementary filter.

First, the current orientation is estimated at time \(t\), from a previous orientation at time \(t-1\), and a given angular velocity, \(\omega\), in rad/s.

This orientation is computed by numerically integrating the angular velocity and adding it to the previous orientation, which is known as an attitude propagation.

\[\begin{split}\begin{array}{rcl} \mathbf{q}_\omega &=& \Big(\mathbf{I}_4 + \frac{\Delta t}{2}\boldsymbol\Omega_t\Big)\mathbf{q}_{t-1} \\ &=& \begin{bmatrix} 1 & -\frac{\Delta t}{2}\omega_x & -\frac{\Delta t}{2}\omega_y & -\frac{\Delta t}{2}\omega_z \\ \frac{\Delta t}{2}\omega_x & 1 & \frac{\Delta t}{2}\omega_z & -\frac{\Delta t}{2}\omega_y \\ \frac{\Delta t}{2}\omega_y & -\frac{\Delta t}{2}\omega_z & 1 & \frac{\Delta t}{2}\omega_x \\ \frac{\Delta t}{2}\omega_z & \frac{\Delta t}{2}\omega_y & -\frac{\Delta t}{2}\omega_x & 1 \end{bmatrix} \begin{bmatrix}q_w \\ q_x \\ q_y \\ q_z \end{bmatrix} \\ &=& \begin{bmatrix} q_w - \frac{\Delta t}{2} \omega_x q_x - \frac{\Delta t}{2} \omega_y q_y - \frac{\Delta t}{2} \omega_z q_z\\ q_x + \frac{\Delta t}{2} \omega_x q_w - \frac{\Delta t}{2} \omega_y q_z + \frac{\Delta t}{2} \omega_z q_y\\ q_y + \frac{\Delta t}{2} \omega_x q_z + \frac{\Delta t}{2} \omega_y q_w - \frac{\Delta t}{2} \omega_z q_x\\ q_z - \frac{\Delta t}{2} \omega_x q_y + \frac{\Delta t}{2} \omega_y q_x + \frac{\Delta t}{2} \omega_z q_w \end{bmatrix} \end{array}\end{split}\]

Secondly, the tilt is computed from the accelerometer measurements as:

\[\begin{split}\begin{array}{rcl} \theta &=& \mathrm{arctan2}(a_y, a_z) \\ \phi &=& \mathrm{arctan2}\big(-a_x, \sqrt{a_y^2+a_z^2}\big) \end{array}\end{split}\]

Only the pitch, \(\phi\), and roll, \(\theta\), angles are computed, leaving the yaw angle, \(\psi\) equal to zero.

If a magnetometer sample is available, the yaw angle can be computed. First compensate the measurement using the tilt:

\[\begin{split}\begin{array}{rcl} \mathbf{b} &=& \begin{bmatrix} \cos\theta & \sin\theta\sin\phi & \sin\theta\cos\phi \\ 0 & \cos\phi & -\sin\phi \\ -\sin\theta & \cos\theta\sin\phi & \cos\theta\cos\phi \end{bmatrix} \begin{bmatrix}m_x \\ m_y \\ m_z\end{bmatrix} \\ \begin{bmatrix}b_x \\ b_y \\ b_z\end{bmatrix} &=& \begin{bmatrix} m_x\cos\theta + m_y\sin\theta\sin\phi + m_z\sin\theta\cos\phi \\ m_y\cos\phi - m_z\sin\phi \\ -m_x\sin\theta + m_y\cos\theta\sin\phi + m_z\cos\theta\cos\phi \end{bmatrix} \end{array}\end{split}\]

Then, the yaw angle, \(\psi\), is obtained as:

\[\begin{split}\begin{array}{rcl} \psi &=& \mathrm{arctan2}(-b_y, b_x) \\ &=& \mathrm{arctan2}\big(m_z\sin\phi - m_y\cos\phi, \; m_x\cos\theta + \sin\theta(m_y\sin\phi + m_z\cos\phi)\big) \end{array}\end{split}\]

We transform the roll-pitch-yaw angles to a quaternion representation:

\[\begin{split}\mathbf{q}_{am} = \begin{pmatrix}q_w\\q_x\\q_y\\q_z\end{pmatrix} = \begin{pmatrix} \cos\Big(\frac{\phi}{2}\Big)\cos\Big(\frac{\theta}{2}\Big)\cos\Big(\frac{\psi}{2}\Big) + \sin\Big(\frac{\phi}{2}\Big)\sin\Big(\frac{\theta}{2}\Big)\sin\Big(\frac{\psi}{2}\Big) \\ \sin\Big(\frac{\phi}{2}\Big)\cos\Big(\frac{\theta}{2}\Big)\cos\Big(\frac{\psi}{2}\Big) - \cos\Big(\frac{\phi}{2}\Big)\sin\Big(\frac{\theta}{2}\Big)\sin\Big(\frac{\psi}{2}\Big) \\ \cos\Big(\frac{\phi}{2}\Big)\sin\Big(\frac{\theta}{2}\Big)\cos\Big(\frac{\psi}{2}\Big) + \sin\Big(\frac{\phi}{2}\Big)\cos\Big(\frac{\theta}{2}\Big)\sin\Big(\frac{\psi}{2}\Big) \\ \cos\Big(\frac{\phi}{2}\Big)\cos\Big(\frac{\theta}{2}\Big)\sin\Big(\frac{\psi}{2}\Big) - \sin\Big(\frac{\phi}{2}\Big)\sin\Big(\frac{\theta}{2}\Big)\cos\Big(\frac{\psi}{2}\Big) \end{pmatrix}\end{split}\]

Finally, after each orientation is estimated independently, they are fused with the complementary filter.

\[\mathbf{q} = (1 - \alpha) \mathbf{q}_\omega + \alpha\mathbf{q}_{am}\]

where \(\mathbf{q}_\omega\) is the attitude estimated from the gyroscope, \(\mathbf{q}_{am}\) is the attitude estimated from the accelerometer and the magnetometer, and \(\alpha\) is the gain of the filter.

The filter gain must be a floating value within the range \([0.0, 1.0]\). It can be seen that when \(\alpha=1\), the attitude is estimated entirely with the accelerometer and the magnetometer. When \(\alpha=0\), it is estimated solely with the gyroscope. The values within the range decide how much of each estimation is “blended” into the quaternion.

This is actually a simple implementation of LERP commonly used to linearly interpolate quaternions with small differences between them.

class ahrs.filters.complementary.Complementary(gyr: numpy.ndarray = None, acc: numpy.ndarray = None, mag: numpy.ndarray = None, frequency: float = 100.0, gain=0.1, **kwargs)¶

Complementary filter for attitude estimation as quaternion.

Parameters:

gyr (numpy.ndarray, default: None) – N-by-3 array with measurements of angular velocity, in rad/s.
acc (numpy.ndarray, default: None) – N-by-3 array with measurements of acceleration, in m/s^2.
mag (numpy.ndarray, default: None) – N-by-3 array with measurements of magnetic field, in mT.
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.
gain (float, default: 0.1) – Filter gain.
q0 (numpy.ndarray, default: None) – Initial orientation, as a versor (normalized quaternion).

Raises:

ValueError – When dimension of input arrays acc, gyr, or mag are not equal.

am_estimation(acc: numpy.ndarray, mag: numpy.ndarray = None) → numpy.ndarray¶

Attitude estimation from an Accelerometer-Magnetometer architecture.

First estimate the tilt from a given accelerometer sample \(\mathbf{a}=\begin{bmatrix}a_x & a_y & a_z\end{bmatrix}^T\) as:

\[\begin{split}\begin{array}{rcl} \theta &=& \mathrm{arctan2}(a_y, a_z) \\ \phi &=& \mathrm{arctan2}\big(-a_x, \sqrt{a_y^2+a_z^2}\big) \end{array}\end{split}\]

Then the yaw angle, \(\psi\), is computed, if a magnetometer sample \(\mathbf{m}=\begin{bmatrix}m_x & m_y & m_z\end{bmatrix}^T\) is available:

\[\psi = \mathrm{arctan2}(-b_y, b_x)\]

where

\[\begin{split}\begin{array}{rcl} b_x &=& m_x\cos\theta + m_y\sin\theta\sin\phi + m_z\sin\theta\cos\phi \\ b_y &=& m_y\cos\phi - m_z\sin\phi \end{array}\end{split}\]

And the roll-pitch-yaw angles are transformed to a quaternion that is then returned:

\[\begin{split}\mathbf{q}_{am} = \begin{pmatrix}q_w\\q_x\\q_y\\q_z\end{pmatrix} = \begin{pmatrix} \cos\Big(\frac{\phi}{2}\Big)\cos\Big(\frac{\theta}{2}\Big)\cos\Big(\frac{\psi}{2}\Big) + \sin\Big(\frac{\phi}{2}\Big)\sin\Big(\frac{\theta}{2}\Big)\sin\Big(\frac{\psi}{2}\Big) \\ \sin\Big(\frac{\phi}{2}\Big)\cos\Big(\frac{\theta}{2}\Big)\cos\Big(\frac{\psi}{2}\Big) - \cos\Big(\frac{\phi}{2}\Big)\sin\Big(\frac{\theta}{2}\Big)\sin\Big(\frac{\psi}{2}\Big) \\ \cos\Big(\frac{\phi}{2}\Big)\sin\Big(\frac{\theta}{2}\Big)\cos\Big(\frac{\psi}{2}\Big) + \sin\Big(\frac{\phi}{2}\Big)\cos\Big(\frac{\theta}{2}\Big)\sin\Big(\frac{\psi}{2}\Big) \\ \cos\Big(\frac{\phi}{2}\Big)\cos\Big(\frac{\theta}{2}\Big)\sin\Big(\frac{\psi}{2}\Big) - \sin\Big(\frac{\phi}{2}\Big)\sin\Big(\frac{\theta}{2}\Big)\cos\Big(\frac{\psi}{2}\Big) \end{pmatrix}\end{split}\]

Parameters:	acc (numpy.ndarray) – Tri-axial sample of the accelerometer. mag (numpy.ndarray, default: None) – Tri-axial sample of the magnetometer.
Returns:	q_am – Estimated attitude.
Return type:	numpy.ndarray

attitude_propagation(q: numpy.ndarray, omega: numpy.ndarray) → numpy.ndarray¶

Attitude propagation of the orientation.

Estimate the current orientation at time \(t\), from a given orientation at time \(t-1\) and a given angular velocity, \(\omega\), in rad/s.

It is computed by numerically integrating the angular velocity and adding it to the previous orientation.

\[\begin{split}\mathbf{q}_\omega = \begin{bmatrix} q_w - \frac{\Delta t}{2} \omega_x q_x - \frac{\Delta t}{2} \omega_y q_y - \frac{\Delta t}{2} \omega_z q_z\\ q_x + \frac{\Delta t}{2} \omega_x q_w - \frac{\Delta t}{2} \omega_y q_z + \frac{\Delta t}{2} \omega_z q_y\\ q_y + \frac{\Delta t}{2} \omega_x q_z + \frac{\Delta t}{2} \omega_y q_w - \frac{\Delta t}{2} \omega_z q_x\\ q_z - \frac{\Delta t}{2} \omega_x q_y + \frac{\Delta t}{2} \omega_y q_x + \frac{\Delta t}{2} \omega_z q_w \end{bmatrix}\end{split}\]

Parameters:	q (numpy.ndarray) – A-priori quaternion. omega (numpy.ndarray) – Tri-axial angular velocity, in rad/s.
Returns:	q_omega – Estimated orientation, as quaternion.
Return type:	numpy.ndarray

update(q: numpy.ndarray, gyr: numpy.ndarray, acc: numpy.ndarray, mag: numpy.ndarray = None) → numpy.ndarray¶

Attitude Estimation from given measurements and previous orientation.

The new orientation is first estimated with the angular velocity, then another orientation is computed using the accelerometers and magnetometers. The magnetometer is optional.

Each orientation is estimated independently and fused with a complementary filter.

\[\mathbf{q} = (1 - \alpha) \mathbf{q}_\omega + \alpha\mathbf{q}_{am}\]

Parameters:	q (numpy.ndarray) – A-priori quaternion. gyr (numpy.ndarray) – Sample of tri-axial Gyroscope in rad/s. acc (numpy.ndarray) – Sample of tri-axial Accelerometer in m/s^2. mag (numpy.ndarray, default: None) – Sample of tri-axial Magnetometer in uT.
Returns:	q – Estimated quaternion.
Return type:	numpy.ndarray