Complementary Filter#

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

The complementary filter is one of the simplest ways to fuse sensor data from multiple sensors. It is based on the idea that the errors from one sensor will be compensated by the other sensor, and vice versa.

The gyroscopes tend to have a low-frequency drift, while the accelerometers and magnetometers tend to have a high-frequency noise. The complementary filter simple “combines” these two signals, yielding the benefit of eliminating the drift from the gyroscope and noise from the accelerometer.

First, the tilt angles computed from the accelerometer measurements as:

\[\begin{split}\boldsymbol{\theta}_{am} = \begin{bmatrix} \theta_x \\ \theta_y \\ \theta_z \end{bmatrix} = \begin{bmatrix} \mathrm{arctan2}(a_y, a_z) \\ \mathrm{arctan2}\big(-a_x, \sqrt{a_y^2+a_z^2}\big) \\ 0 \end{bmatrix}\end{split}\]

Only the roll, \(\theta_x\), and pitch, \(\theta_y\), angles are computed, leaving the yaw angle, \(\theta_z\), equal to zero.

If a magnetometer sample is available, the yaw angle, \(\theta_z\) can be computed. First compensate the measured magnetic field using the tilt:

\[\begin{split}\begin{array}{rcl} \mathbf{b} &=& \begin{bmatrix} \cos\theta_x & \sin\theta_x\sin\theta_y & \sin\theta_x\cos\theta_y \\ 0 & \cos\theta_y & -\sin\theta_y \\ -\sin\theta_x & \cos\theta_x\sin\theta_y & \cos\theta_x\cos\theta_y \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_x + m_y\sin\theta_x\sin\theta_y + m_z\sin\theta_x\cos\theta_y \\ m_y\cos\theta_y - m_z\sin\theta_y \\ -m_x\sin\theta_x + m_y\cos\theta_x\sin\theta_y + m_z\cos\theta_x\cos\theta_y \end{bmatrix} \end{array}\end{split}\]

Then \(\theta_z\) is obtained as:

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

Likewise, the orientation is again estimated at \(t\), but this time from a previous orientation at \(t-1\), and a given angular velocity, \(\boldsymbol{\omega}=\begin{bmatrix}\omega_x & \omega_y & \omega_z\end{bmatrix}^T\), in rad/s, using the simplest numerical integration:

\[\begin{split}\boldsymbol{\theta}_\omega = \begin{bmatrix} \theta_{x_t} \\ \theta_{y_t} \\ \theta_{z_t} \end{bmatrix} = \begin{bmatrix} \theta_{x_{t-1}} + \omega_x\Delta_t \\ \theta_{y_{t-1}} + \omega_y\Delta_t \\ \theta_{z_{t-1}} + \omega_z\Delta_t \end{bmatrix}\end{split}\]

where \(\Delta_t\) is the time interval between the current and previous measurements, a.k.a. the sampling period or time step. This is merely the opposite of the sampling frequency: \(\Delta_t=^1/f_s\).

Finally, the estimations are merged using a complementary filter with a controlling parameter, \(\alpha\), in the range \([0, 1]\):

\[\boldsymbol{\theta} = \alpha\boldsymbol{\theta}_\omega + (1 - \alpha)\boldsymbol{\theta}_{am}\]

where \(\boldsymbol{\theta}_\omega\) is the attitude estimated from the gyroscope, \(\boldsymbol{\theta}_{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=0\), the attitude is estimated entirely with the accelerometer and the magnetometer. When \(\alpha=1\), it is estimated solely with the gyroscope. The values within the range decide how much of each estimation is “blended” into the quaternion.

class ahrs.filters.complementary.Complementary(gyr: ndarray | None = None, acc: ndarray | None = None, mag: ndarray | None = 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.9) – Filter gain. Gain equal to 1 uses Angular velocities (gyroscopes) only. Gain equal to 0 uses Accelerometer, and Magnetometer if available, only. Values greater than zero and less than one blend the two estimations proportionally.

  • w0 (numpy.ndarray, default: None) – Initial angular position, as roll-pitch-yaw angles, in radians.

  • q0 (numpy.ndarray, default: None) – Initial orientation, as a versor (normalized quaternion).

  • representation (str, default: 'angles') – Attitude representation. Options are 'angles', 'quaternion' or 'rotmat'.

Raises:

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

property Q: ndarray#

Attitudes as quaternions.

Returns:

Q – M-by-4 Array with all estimated quaternions, where M is the number of samples.

Return type:

numpy.ndarray

am_estimation(acc: ndarray, mag: ndarray | None = None) → ndarray#

Attitude estimation from an Accelerometer-Magnetometer architecture.

Parameters:

  • acc (numpy.ndarray) – N-by-3 array with measurements of gravitational acceleration.

  • mag (numpy.ndarray, default: None) – N-by-3 array with measurements of local geomagnetic field.

Returns:

q_am – Estimated attitude.

Return type:

numpy.ndarray