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: float = 0.9, **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.

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, dt: float) → 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.

Parameters:
  • q (numpy.ndarray) – A-priori quaternion.
  • omega (numpy.ndarray) – Tri-axial angular velocity, in rad/s.
  • dt (float) – Time step, in seconds, between consecutive Quaternions.
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, dt: float = 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.
  • dt (float, default: None) – Time step, in seconds, between consecutive Quaternions.
Returns:

q – Estimated quaternion.

Return type:

numpy.ndarray