How Does IMU Sensor Fusion Work?
Posted on April 3, 2023 by Zach Strout
Why do we need sensor fusion?
Relationship between roll, pitch, and yaw - GregorDS, 6DOF, CC BY-SA 4.0
Inertial measurement units (IMUs) typically contain accelerometer, gyroscope, and magnetometer sensors. While these individual sensors can measure a variety of movement parameters (e.g. peak tibial acceleration from accelerometers, gait events from gyroscopes), the true power of IMUs lies in fusing the sensor data to magnify the strengths of each sensor. Computing IMU orientation in 3D space as roll, pitch, and yaw or as a quaternion representing the orientation involves considering the pros and cons of each sensor type. For example, accelerometers can easily determine roll and pitch using the downward acceleration gravity vector for slower movements. However, accelerometers suffer from noise interference, where small vibrations or movements can impact the roll and pitch angle estimations. Any small vibrations or movements can interfere with the roll and pitch angle estimations. For highly dynamic movements, sometimes the acceleration due to gravity can be small relative to the acceleration due to movement, so pitch and roll will be difficult to estimate. Also, yaw is impossible to measure with accelerometers alone since there are no absolute references that accelerometers can use.
Yaw angle changes (rotation about the gravity vector) can not be estimated from IMU accelerometer signals.
While it might seem at first that gyroscopes that measure angular velocity can not estimate orientation, this can be done many ways including a technique called strapdown integration. More simply, while the rate equation $(rate\cdot \Delta time = \Delta distance)$ is for linear movement, there is an equivalent equation for rotational movement $(angular\space rate \cdot \Delta time = \Delta angle)$. This allows for roll, pitch, and yaw to be estimated by adding this change ($\Delta$) in angle to the current angle every sample like $angle_{t+1}=angle_t+\Delta angle$. One of the major problems with gyroscopes is that they are prone to drift. When strapdown integration is used, a little error is added at every estimation step. In the end, all this error can add up so that the final estimated angle after some time is completely wrong. While constant bias is relatively easy to remove from the signal, some IMUs have bias drift meaning that bias the bias changes over time, so it can not simply be measured from a static IMU and subtracted.
Orientation drift when only gyroscope is used. Angle is calculated with $angle_t = angle_{t-1}+gyro_{t}\cdot\Delta t $.
Magnetometers can be used to determine part of the orientation of the IMU too. The magnetic north pole acts as an absolute reference for the orientation of the IMU, and the math used to calculate this is relatively trivial. The major problems with magnetometers are electromagnetic interference and unconstrained rotation of the IMU around the earth’s magnetic field line vectors. When there are magnetic or ferromagnetic things in the vicinity of the IMU, the magnetic fields from the earth can be distorted and the orientation estimation can be incorrect. Also, if the IMU is rotated around a vector representing the magnetic field lines, there will be no change in the magnetometer reading meaning that the orientation could not be represented solely by the magnetometer.
Angle changes about magnetic field lines can not be estimated from magnetometer signals.
The fusion of data from these three sensors can significantly remove most of the errors, biases, and unconstrained dimensions. For instance, we can use the smooth signal of the gyroscope to compensate for the noisy signal in the accelerometer. Furthermore, we can compensate for the drift in the gyroscope by referencing absolute measurements obtained from the accelerometer and magnetometer. To address the problem of lacking yaw reference for accelerometer-based orientation, we can leverage the magnetometer, while the accelerometer can help solve the issue of missing reference for the axis about the magnetic field lines for magnetometer-based orientation. Ultimately, this allows us to estimate a robust orientation of the IMU and utilize relative orientations between IMUs on different body segments to estimate various things such as joint angles.
How do you "fuse" the IMU sensor data together?
Given that each sensor is good at different things, how do you combine the sensors in a way that maximizes the benefit of each sensor? There are many different sensor fusion algorithms, we will look at three commonly used methods: complementary filters, Kalman filters, and the Madgwick algorithm.
Complementary Filter
The idea behind complementary filters is that the sensors are added in a way that complements each other. This is done by using a weighted average after filtering the data with both a high pass filter on the gyroscope and a low pass filter for the accelerometer. If we were to fuse the accelerometer and gyroscope signals, this could be done with the following equation where $\alpha$ (alpha) is a weighting factor from 0 to 1:
$$angle = (1-\alpha)\cdot (gyro \space angle) + \alpha \cdot accelerometer \space angle $$
When $\alpha$ is 0.5, this is a simple average between these two values as is shown below. This is not ideal though. If $\alpha$ is too small, you are just using the accelerometer to estimate the angle which is noisy. If $\alpha$ is too large, the estimate from the gyroscope will cause the overall estimate to diverge from the true value for the angle.
$$angle = 0.5\cdot gyro \space angle + 0.5 \cdot accelerometer \space angle$$
$$= \frac{gyro \space angle + accelerometer \space angle}{2} $$
It's best to select $\alpha$ such that the angle does not diverge while also being more responsive to dynamic signals. This selection is typically done experimentally by trying many alpha values until the one that minimizes the error is selected. One of the key things to note is that this $\alpha$ value is constant. If the data becomes more or less reliable, $\alpha$ does not change to compensate. The next sensor fusion method that we'll discuss directly deals with this problem.
Kalman Filter
Kalman filters are somewhat like complementary filters except that they are a bit more formal in their structure of the problem that they are trying to solve. For example, instead of assuming that the measurement is equal to the true value, Kalman filters assume that there is some sort of noise in the measurement. In fact, they assume that the variables of interest can often not be directly measured and are defined as a set of variables known as "state variables". Furthermore, the Kalman filter often describes not the state variables directly but only an estimate of the state variables to have a further distinction between the measurement and the fundamental properties that we are trying to figure out with the Kalman filter. Beyond estimating these state variables, the Kalman filter also keeps track of the variance in these state variable estimates as a measure of how accurate the estimate is.
One of the reasons that Kalman filters are difficult for many people to understand is that they require an understanding of the physics of the system since Kalman filters not only estimate the current state but also predict how the state changes from one time step to another. You need to know how to describe this transformation from one time step to another. Traditional Kalman filters also require the transformation to be relatively simple (linear). In fact, the mathematics behind orientation calculations is too complex for Kalman filters since orientation calculations require trigonometry functions, and the variables for the equations combine in non-linear ways. There is a form of the Kalman filter that is able to work for complex state transformations (Extended Kalman Filters), but this is more complex to implement and it is very sensitive to the initial parameters and assumptions that are made. The Kalman filter also requires an understanding of how the sensor values are transformed into state variables. This transformation is where the sensor fusion part is implemented for Kalman filters.
Beyond the physics of the system, it is also important to have a clear understanding of the sources of noise in the system such as process noise and measurement noise. Process noise comes from phenomena that are inherent to the system like noise due to temperature fluctuations or general uncertainty in how the system is modeled. Measurement noise is noise specifically in the measurement device and is a factor determining how reliable the measurement device is. These two types of noise basically determine how much the filter should trust the model and how much it should trust the measurement. The Kalman filter takes account of this noise and the variance in the state in order to calculate the Kalman gain which is like a dynamic weight to select between trusting the model more or the measurements more.
There are many great resources to understand how Kalman filters work: Kalmanfilter.net is a great website that includes numerical examples of how Kalman filters work. This paper about Kalman filters has pretty rigorous mathematics, but it clearly explains how the state variance matrix changes over time and how the mathematics is derived. This tutorial also is really helpful in simply explaining Kalman filters and even extended Kalman filters.
Madgwick Algorithm
The Madgwick algorithm uses a quaternion-based approach to estimate the orientation of the object. The basic idea of the algorithm is to minimize the error between the measured data and the estimated data using a gradient descent optimization method. This involves iteratively adjusting the orientation estimate until the error is minimized. The optimization process starts with the accelerometer and magnetometer measurements, which are used to estimate the orientation of the object with absolute references to orientation. The gyroscope measurements are then used to update the orientation estimate over time. The Madgwick filter combines the orientation related to the gyroscope with the two other orientations with a complementary filter. The algorithm also includes a step to estimate the sensor biases, which can be a significant source of error in the orientation estimate. Beyond fusing sensors, the Madgwick algorithm also features methods to compensate for magnetic distortions and gyroscope bias drift. The magnetic distortion compensation is performed by calibrating the magnetometer signal with the accelerometer. The gyroscope bias drift compensation is performed by integrating the estimated gyroscope error and subtracting this from the raw gyroscope values.
Find Out More
Sometimes the easiest way to understand how an algorithm works is to see how it is implemented in code. The AHRS python package does a great job of explaining all of the common algorithms both theoretically in the documentation and practically in python code.
There are also lots of great papers about sensor fusion methods:
Two step complementary filter to improve IMU orientation accuracy
