On filters and AHRS systems
In this post we go thorough basics of attitude and heading reference systems (AHRS) and dicuss in detail the Madgwick’s algorithm (MA) which is an optimized solution for pose estimation. Note that the use of terms filter and algorithm become intertwined and their differences are left for the reader to decipher.
Introduction
In a recent project I was introduced to inertial measurement units (IMU) that eventually led to inspect AHRS in greater detail. An IMU integrated circuit often cosist of accelerometer and a gyroscope. Some ICs include a magnetometer. I will not go into the details of these sensor types here. For the curios I recommend a fantastic python library1 that describes the bigger picture very well.
If you are like me you might like to try to see common factors accross seemingly different topics. I think it adds a layer of challenge and makes learning new areas both fun and rewarding. Hence I like the reader to think of traditional digital filters in mind when getting to know AHRS. In AHRS there are several ways to accomplish pose estimation with different sensor types. For quick reference: a rigid body’s pose can be estimated in a local or in global frame. Earth and compass points are one example of a global frame. We can denote earth frame as $g_e$. For a newcomer a suitable first pose estimation filter (PEF) to inspect is the complementary filter. In a sense it can be thought of as a special digital filter.
The name for complementary filter (CF) is assumed to come from the fact that the CF uses two or three different sensor to complement one another for better overall pose estimation performance. The equation for CF is expressed as follows:
$$ \theta_c = \alpha \cdot \theta_{\omega} + (1-\alpha) \cdot \theta_a $$
Often times for clarity we define the true pose as $\theta$ and the estimated pose as $\hat{\theta}$. Here we assume all values to be estimations of the true values.
In the equation $\theta_c$ is the filter output, $\alpha$ is the filter weighting coefficient, $\theta_a$ accelerometer pose estimation and $\theta_{\omega}$ gyroscope pose estimation. If $\alpha = \frac{1}{2}$ then the filter takes the average of the respected sensor values. Note that a filter with a magnetometer the $\theta_a$ is replaced by $\theta_{am}$.
$$ \begin{split}{\theta}_{a} = \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} $$
Both accelerometers and gyroscopes suffer from their own “flaws” and when combined with the equation before we have a more accurate and reliable estimation of pose. The alpha term should look familiar to those familiar with digital filters.
$$ y_n = \alpha \cdot x_n + (1-\alpha) \cdot y_{n-1} $$
I want to point out that looking at the equations they don’t seem that different from one another. In the low-pass filter equation above $x_n$ is the most recent sensor value, $y_n$ is the updated filter value and $y_{n-1}$ is the previous filter value. Adjusting the $\alpha$ value makes the system favor either the long-term filter value of the recent sensor values. One way to think about this is that the filter has a longer history.
In the complementary filter there is no implication of system memory. We can think that the complementary filter favors either one pose estimation source over another based on the alpha term. Now just as an exercise think of the benefits of using a CF with a memory of past states. What could be gained from it?
More sophisticated filters
It may come as a no surprise but Kalman filters are also used for PEF. Since it combines three different sensors (magnetometer as a new one) the equations get quickly menacing 2. The ability to add model noise and process noise to the filter has it’s benefits when using different grade sensors. As respectable as the solution is instead let’s focus on a different approach made by Sebastian O.H. Madgwick.
In his thesis Madgwick uses gradient descent as a PEF. To put briefly the pose estimation is constructed as a loss function optimization problem that uses complex values called quaternions. A quaternion is defined:
$$ q = a + b\hat{i} + c\hat{j} + d\hat{k} $$
and
$$ i^2 = j^2 = k^2 = ijk = -1 $$
A three-dimentional rotation is described as a quaternion multiplication $f(p) = q \cdot p \cdot q^{-1}$. In MA by loss function definition we try to find a quaternion that rotates the pose estimation based on sensor values to point to the direction of earth frame $g_e$. The pose estimation is described by function $f(q, g_s, g_e)$. Since hugo sites have a problem with rendering matrices and equations spanning mutile lines I won’t include all equations here. For full equations see 3.
TBC
A concept might have come to your mind when reading this post: sensor fusion. In essence this is what AHRS systems are about.